

/********************************************************************
* Description: control.c
*   emcmotController() is the main loop running at the servo cycle
*   rate. All state logic and trajectory calcs are called from here.
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* Created on:
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/

#include "posemath.h"
#include "rtapi.h"
#include "hal.h"
#include "emcmotglb.h"
#include "motion.h"
#include "mot_priv.h"
#include "math.h"
#include "tp.h"
//#include "tc.h"
#include "motion_debug.h"
#include "config.h"
#include "motion_types.h"
#include "emcmotionclass.h"
#include "usrmotintf.h"
#include <vector>

// Mark strings for translation, but defer translation to userspace
#define _(s) (s)

#include "emccontrolclass.h"
#include "cubicclass.h"
#include "emccommandclass.h"
//#include <QDebug>
#include<iostream>
#include <carvingmodule.h>
#include "wavemotion.h"

using namespace std;//此句也可以在main函数体中出现。

//#define D_VEL_FILTER_SLOW_RATIO 0.7
extern double g_waveMonitorData[D_WAVE_AXIS_SIZE];

EmcControlClass::EmcControlClass(CarvingModule *cavingModuleIn, double arm_warnSlowRatioIn,
                                 double cycleIn, MotionMessage *motionMessageIn,
                                 double maxLineVelIn, double maxAccIn,int robotIdIn)
{
    cavingModule=cavingModuleIn;
    interpoStatus=D_INTERPOLSTION_STATUS_NORMAL;
    arm_warnSlowRatio=arm_warnSlowRatioIn;
    robotId=robotIdIn;

    /* calculate servo period as a double - period is in integer nsec */
    servo_period = cycleIn;//unit is s
    servo_freq = 1.0 / servo_period;
    waveMotionControl=new WaveMotion(cycleIn,motionMessageIn,maxLineVelIn,maxAccIn);
}

int EmcControlClass::resetAll()
{
    waveMotionControl->resetAll();
    return 1;
}

static void printPose(EmcPose poseIn)
{
    std::cout<<"pose"<<poseIn.tran.x<<" "<<poseIn.tran.y<<" "<<poseIn.tran.z<<" "
            <<poseIn.a<<" "<<poseIn.b<<" "<<poseIn.c<<std::endl;

}
//extern int abort_and_switchback(void); // command.c

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

/*! \todo FIXME - this is a leftover global, it will eventually go away */
//int rehomeAll;

/* the (nominal) period the last time the motion handler was invoked */
unsigned long last_period = 0;

/* these variables have the servo cycle time and 1/cycle time */
double servo_period;
double servo_freq;


/*! \todo FIXME - debugging - uncomment the following line to log changes in
   JOINT_FLAG and MOTION_FLAG */
// #define WATCH_FLAGS 1

// ignore jog moves during pause if feed too low
#define MINIMUM_JOG_VELOCITY 0.1 // FIXME: questionable


/* debugging function - it watches a particular variable and
   prints a message when the value changes.  Right now there are
   calls to this scattered throughout this and other files.
   To disable them, comment out the following define:
*/
// #define ENABLE_CHECK_STUFF

#ifdef ENABLE_CHECK_STUFF
void check_stuff(const char *location)
{
   static char *target, old = 0xFF;
/*! \todo Another #if 0 */
#if 0
/* kludge to look at emcmotDebug[robotId]->enabling and emcmotStatus[robotId]->motionFlag
   at the same time - we simply use a high bit of the flags to
   hold "enabling" */
   short tmp;
   if ( emcmotDebug[robotId]->enabling )
     tmp = 0x1000;
   else
     tmp = 0x0;
   tmp |= emcmotStatus[robotId]->motionFlag;
   target = &tmp;
/* end of kluge */
#endif

    target = (emcmot_hal_data->enable);
    if ( old != *target ) {
    rtapi_print ( "%d: watch value %02X (%s)\n", emcmotStatus[robotId]->heartbeat, *target, location );
    old = *target;
    }
}
#else /* make it disappear */
void check_stuff(const char *location)
{
/* do nothing (I wonder if gcc is smart
   enough to optimize the calls away?) */
}
#endif /* ENABLE_CHECK_STUFF */


/***********************************************************************
*                      LOCAL FUNCTION PROTOTYPES                       *
************************************************************************/

/* the following functions are called (in this order) by the main
   controller function.  They are an attempt to break the huge
   function (originally 1600 lines) into something a little easier
   to understand.
*/



/* 'do forward kins()' takes the position feedback in joint coords
   and applies the forward kinematics to it to generate feedback
   in Cartesean coordinates.  It has code to handle machines that
   don't have forward kins, and other special cases, such as when
   the joints have not been homed.
*/
static void do_forward_kins(void);



/* 'check_for_faults()' is responsible for detecting fault conditions
   such as limit switches, amp faults, following error, etc.  It only
   checks active axes.  It is also responsible for generating an error
   message.  (Later, once I understand the cmd/status/error interface
   better, it will probably generate error codes that can be passed
   up the architecture toward the GUI - printing error messages
   directly seems a little messy)
*/
static void check_for_faults(void);

/* 'set_operating_mode()' handles transitions between the operating
   modes, which are free, coordinated, and teleop.  This stuff needs
   to be better documented.  It is basically a state machine, with
   a current state, a desired state, and rules determining when the
   state can change.  It should be rewritten as such, but for now
   it consists of code copied exactly from emc1.
*/
static void set_operating_mode(void);



/* 'do_homing_sequence()' looks at emcmotStatus[robotId]->homingSequenceState
   to decide what, if anything, needs to be done related to multi-joint
   homing.

   no prototype here, implemented in homing.c, proto in mot_priv.h
 */

/* 'do_homing()' looks at the home_state field of each joint struct
    to decide what, if anything, needs to be done related to homing
    the joint.  Homing is implemented as a state machine, the exact
    sequence of states depends on the machine configuration.  It
    can be as simple as immediately setting the current position to
    zero, or a it can be a multi-step process (find switch, set
    approximate zero, back off switch, find index, set final zero,
    rapid to home position), or anywhere in between.

   no prototype here, implemented in homing.c, proto in mot_priv.h
*/










/***********************************************************************
*                        PUBLIC FUNCTION CODE                          *
************************************************************************/

/*
  emcmotController() runs the trajectory and interpolation calculations
  each control cycle

  This function gets called at regular intervals - therefore it does NOT
  have a loop within it!

  Inactive axes are still calculated, but the PIDs are inhibited and
  the amp enable/disable are inhibited
  */

int EmcControlClass::emcmotController()
{
    long period ;// 单位ns TrajCycleTime

    // - overrun detection -
    // maintain some records of how long it's been between calls.  The
    // first time we see a delay that's much longer than the records show
    // is normal, report an error.  This might detect bogus realtime
    // performance caused by ACPI, onboard video, etc.  It can be reliably
    // triggered by maximizing glxgears on my nvidia system, which also
    // causes the rtai latency test to show overruns.

    // check below if you set this under 5
#define CYCLE_HISTORY 5


    static long long int last = 0;
    static int index = 0;
    static long int cycles[CYCLE_HISTORY];
    static int priming = 1;

    long long int now ;//= fa_start_time(fa)
    long int this_run = (long int)(now - last);
    emcmot_hal_data->last_period = this_run;
#ifdef HAVE_CPU_KHZ
    emcmot_hal_data->last_period_ns = this_run * 1e6 / cpu_khz;
#endif

    if(!priming ) {
        // we have CYCLE_HISTORY samples, so check for this call being
        // anomolously late
        int i;

        for(i=0; i<CYCLE_HISTORY; i++) {
            if (this_run > 1.2 * cycles[i]) {
                emcmot_hal_data->overruns++;
                // print message on first overrun only
                if(emcmot_hal_data->overruns == 1) {
//                    int saved_level = rtapi_get_msg_level();
//                    rtapi_set_msg_level(RTAPI_MSG_ALL);
//                    tp_debug_print(_("Unexpected realtime delay: check dmesg for details."));
//                    rtapi_print_msg(RTAPI_MSG_WARN,
//                        _("\nIn recent history there were\n"
//                        "%ld, %ld, %ld, %ld, and %ld\n"
//                        "elapsed clocks between calls to the motion controller.\n"),
//                        cycles[0], cycles[1], cycles[2], cycles[3], cycles[4]);
//                    rtapi_print_msg(RTAPI_MSG_WARN,
//                        _("This time, there were %ld which is so anomalously\n"
//                        "large that it probably signifies a problem with your\n"
//                        "realtime configuration.  For the rest of this run of\n"
//                        "EMC, this message will be suppressed.\n\n"),
//                        this_run);
//                    rtapi_set_msg_level(saved_level);
                }

        break;
        }
        }
    if(last) {
        cycles[index++] = this_run;
    }
    if(index == CYCLE_HISTORY) {
        // wrap around to the start of the array
        index = 0;
        // we now have CYCLE_HISTORY good samples, so start checking times
        priming = 0;
    }
    }
    // we need this for next time
    last = now;

    // end of overrun detection

    /* calculate servo period as a double - period is in integer nsec */
    servo_period = period * 0.000000001;

    if(period != last_period) {
        EmcMotionClass::emcmotSetCycleTime(period,robotId);
        last_period = period;
    }

    /* calculate servo frequency for calcs like vel = Dpos / period */
    /* it's faster to do vel = Dpos * freq */
    servo_freq = 1.0 / servo_period;
    /* increment head count to indicate work in progress */
    emcmotStatus[robotId]->head++;
    /* here begins the core of the controller */

check_stuff ( "before process_inputs()" );
    process_inputs();
check_stuff ( "after process_inputs()" );
    do_forward_kins();
check_stuff ( "after do_forward_kins()" );
    process_probe_inputs();
check_stuff ( "after process_probe_inputs()" );
    check_for_faults();
check_stuff ( "after check_for_faults()" );
    set_operating_mode();
check_stuff ( "after set_operating_mode()" );
    handle_jogwheels();
check_stuff ( "after handle_jogwheels()" );
//    do_homing_sequence();
check_stuff ( "after do_homing_sequence()" );
//    do_homing();
check_stuff ( "after do_homing()" );
    get_pos_cmds(period);
check_stuff ( "after get_pos_cmds()" );
    compute_screw_comp();
check_stuff ( "after compute_screw_comp()" );
    output_to_hal();
check_stuff ( "after output_to_hal()" );
    update_status();
check_stuff ( "after update_status()" );
    /* here ends the core of the controller */
    emcmotStatus[robotId]->heartbeat++;
    /* set tail to head, to indicate work complete */
    emcmotStatus[robotId]->tail = emcmotStatus[robotId]->head;
    /* clear init flag */
//    first_pass = 0;

/* end of controller function */
    return 0;
}



/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/

/* The protoypes and documentation for these functions are located
   at the top of the file in the section called "local function
   prototypes"
*/

void EmcControlClass::process_probe_inputs(void) {
    static int old_probeVal = 0;
    unsigned char probe_type = emcmotStatus[robotId]->probe_type;

    // don't error
    char probe_suppress = probe_type & 1;

    // trigger when the probe clears, instead of the usual case of triggering when it trips
    char probe_whenclears = !!(probe_type & 2);

    /* read probe input */
    emcmotStatus[robotId]->probeVal = !!*(emcmot_hal_data->probe_input);
    if (emcmotStatus[robotId]->probing) {
        /* check if the probe has been tripped */
        if (emcmotStatus[robotId]->probeVal ^ probe_whenclears) {
            /* remember the current position */
            emcmotStatus[robotId]->probedPos = emcmotStatus[robotId]->carte_pos_fb;
            /* stop! */
            emcmotStatus[robotId]->probing = 0;
            emcmotStatus[robotId]->probeTripped = 1;
            EmcCommandClass::abort_and_switchback(robotId); // tpAbort(emcmotQueue[robotId]);
        /* check if the probe hasn't tripped, but the move finished */
        } else if (GET_MOTION_INPOS_FLAG(emcmotStatus[robotId]->motionFlag) &&
           tpQueueDepth(emcmotQueue[robotId]) == 0) {
            /* we are already stopped, but we need to remember the current
               position here, because it will still be queried */
            emcmotStatus[robotId]->probedPos = emcmotStatus[robotId]->carte_pos_fb;
            emcmotStatus[robotId]->probing = 0;
            if (probe_suppress) {
                emcmotStatus[robotId]->probeTripped = 0;
            } else if(probe_whenclears) {
                reportError(_("G38.4 move finished without breaking contact."));
                SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
            } else {
                reportError(_("G38.2 move finished without making contact."));
                SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
            }
        }
    } else if (!old_probeVal && emcmotStatus[robotId]->probeVal) {
        // not probing, but we have a rising edge on the probe.
        // this could be expensive if we don't stop.
        int i;
        int aborted = 0;

        if(!GET_MOTION_INPOS_FLAG(emcmotStatus[robotId]->motionFlag) &&
       tpQueueDepth(emcmotQueue[robotId]) &&
           tpGetExecId(emcmotQueue[robotId]) <= 0) {
            // running an MDI command
            EmcCommandClass::abort_and_switchback(robotId); // tpAbort(emcmotQueue[robotId]);
            reportError(_("Probe tripped during non-probe MDI command."));
        SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
        }

        for(i=0; i<num_joints; i++) {
            emcmot_joint_t *joint = &joints[i+robotId*EMCMOT_MAX_JOINTS];

            if (!GET_JOINT_ACTIVE_FLAG(joint)) {
                /* if joint is not active, skip it */
                continue;
            }

            // abort any homing
            if(GET_JOINT_HOMING_FLAG(joint)) {
                joint->home_state = HOME_ABORT;
                aborted=1;
            }

            // abort any jogs
            if(joint->free_tp_enable == 1) {
                joint->free_tp_enable = 0;
                // since homing uses free_tp, this protection of aborted
                // is needed so the user gets the correct error.
                if(!aborted) aborted=2;
            }
        }

        if(aborted == 1) {
            reportError(_("Probe tripped during homing motion."));
        }

        if(aborted == 2) {
            reportError(_("Probe tripped during a jog."));
        }
    }
    old_probeVal = emcmotStatus[robotId]->probeVal;
}

#define EQUAL_EMC_POSE(p1,p2) \
    (((p1).tran.x == (p2).tran.x) && \
     ((p1).tran.y == (p2).tran.y) && \
     ((p1).tran.z == (p2).tran.z) && \
     ((p1).a == (p2).a) &&		 \
     ((p1).b == (p2).b) &&		 \
     ((p1).c == (p2).c) &&		 \
     ((p1).u == (p2).u) &&		 \
     ((p1).v == (p2).v) &&		 \
     ((p1).w == (p2).w))

//extern int inRange(EmcPose pos, int id, char *move_type); // from command.c

void EmcControlClass::update_offset_pose(void)
{
    EmcPose *nt = &emcmotStatus[robotId]->pause_offset_carte_pos;
    EmcPose *ipp = &emcmotStatus[robotId]->pause_carte_pos;

    nt->tran.x = *emcmot_hal_data->pause_offset_x + ipp->tran.x;
    nt->tran.y = *emcmot_hal_data->pause_offset_y + ipp->tran.y;
    nt->tran.z = *emcmot_hal_data->pause_offset_z + ipp->tran.z;
    nt->a = *emcmot_hal_data->pause_offset_a + ipp->a;
    nt->b = *emcmot_hal_data->pause_offset_b + ipp->b;
    nt->c = *emcmot_hal_data->pause_offset_c + ipp->c;
    nt->u = *emcmot_hal_data->pause_offset_u + ipp->u;
    nt->v = *emcmot_hal_data->pause_offset_v + ipp->v;
    nt->w = *emcmot_hal_data->pause_offset_w + ipp->w;
}

void EmcControlClass::process_inputs(void)
{
    int joint_num;
    double abs_ferror, tmp, scale;
    joint_hal_t *joint_data;
    emcmot_joint_t *joint;
    unsigned char enables;
    /* read spindle angle (for threading, etc) */
    emcmotStatus[robotId]->spindleRevs = *emcmot_hal_data->spindle_revs;
    emcmotStatus[robotId]->spindleSpeedIn = *emcmot_hal_data->spindle_speed_in;
    emcmotStatus[robotId]->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed;
    /* compute net feed and spindle scale factors */
    if ( emcmotStatus[robotId]->motion_state == EMCMOT_MOTION_COORD ) {
    /* use the enables that were queued with the current move */
    enables = emcmotStatus[robotId]->enables_queued;
    } else {
    /* use the enables that are in effect right now */
    enables = emcmotStatus[robotId]->enables_new;
    }
    /* feed scaling first:  feed_scale, adaptive_feed, and feed_hold */
    scale = 1.0;
    if ( enables & FS_ENABLED ) {
        if (emcmotStatus[robotId]->motionType == EMC_MOTION_TYPE_TRAVERSE) {
            scale *= emcmotStatus[robotId]->rapid_scale;
        } else {
            scale *= emcmotStatus[robotId]->feed_scale;
        }
    }
    if ( enables & AF_ENABLED ) {
    /* read and clamp (0.0 to 1.0) adaptive feed HAL pin */
    tmp = *emcmot_hal_data->adaptive_feed;
    if ( tmp > 1.0 ) {
        tmp = 1.0;
    } else if ( tmp < 0.0 ) {
        tmp = 0.0;
    }
    scale *= tmp;
    }
    if ( enables & FH_ENABLED ) {
    /* read feed hold HAL pin */
    if ( *emcmot_hal_data->feed_hold ) {
        scale = 0;
    }
    }
    /*non maskable (except during spinndle synch move) feed hold inhibit pin */
    if ( enables & *emcmot_hal_data->feed_inhibit ) {
        scale = 0;
    }


    // pause FSM
    switch (*emcmot_hal_data->pause_state) {

    case PS_RUNNING: // nothing to do.
    break;

    case PS_PAUSING_FOR_STEP:
    //rtapi_print_msg(RTAPI_MSG_DBG, "paused for step\n");
    tpPause(emcmotQueue[robotId]);
    *emcmot_hal_data->pause_state = PS_PAUSING;;
    break;

    case PS_PAUSING:
    // waiting for tp to actually stop since a spindle-sync motion might be in progress
    if (tpIsPaused(emcmotQueue[robotId])) {
        //rtapi_print_msg(RTAPI_MSG_DBG, "stopped\n");

        // switch to alternate motion queue:

        // record type of paused motion
        *emcmot_hal_data->paused_at_motion_type =
        tpGetMotionType(emcmotQueue[robotId]);

        // record current carte pos for return move
        tpGetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->pause_carte_pos);

        // clone parameters from primary queue
        tpSnapshot(emcmotPrimQueue[robotId], emcmotAltQueue[robotId]);

        // side effect: this also clears emcmotStatus[robotId] fields
        // current_vel, requested_vel, spindleSync, distance_to_go
        tpClear(emcmotAltQueue[robotId]);

        // start alternate queue where we left off the primary queue
        tpSetPos(emcmotAltQueue[robotId],  &emcmotStatus[robotId]->pause_carte_pos);

        // switch to alternate motion queue
        emcmotQueue[robotId] = emcmotAltQueue[robotId];

        // at this point, ready to run motions on alternate queue
        *emcmot_hal_data->pause_state = PS_PAUSED;
    }
    break;

    case PS_PAUSED:
    // on alternate queue, all motion stopped
    // position is the initial pause position, so ok to resume
    if (emcmotStatus[robotId]->resuming) {
        // a resume was signalled.
        // switch to primary queue and resume.
        //rtapi_print_msg(RTAPI_MSG_DBG, "resuming\n");
        emcmotStatus[robotId]->resuming = 0;
        emcmotDebug[robotId]->stepping = 0;
        emcmotQueue[robotId] = emcmotPrimQueue[robotId];
        tpResume(emcmotQueue[robotId]);
         *emcmot_hal_data->pause_state = PS_RUNNING;
         break;
    }
    if (emcmotDebug[robotId]->stepping) {
        //rtapi_print_msg(RTAPI_MSG_DBG, "step signalled - resuming\n");
        emcmotQueue[robotId] = emcmotPrimQueue[robotId];
        tpResume(emcmotQueue[robotId]);
        *emcmot_hal_data->pause_state = PS_RUNNING;
        break;
    }
    // note fall through - check for jog and return motions only once

    case  PS_PAUSED_IN_OFFSET:
    // also PS_PAUSED if not resuming
    // on alternate queue  here, all motion stopped

    if (emcmotStatus[robotId]->resuming || emcmotDebug[robotId]->stepping) {
        // resume, or step was signalled during PS_PAUSED_IN_OFFSET
        // execute return move, which should result in state PS_PAUSED
        //rtapi_print_msg(RTAPI_MSG_DBG, "resuming from PAUSED_IN_OFFSET\n");

        // clamp velocity
        emcmotStatus[robotId]->current_vel = (*emcmot_hal_data->current_vel) =
        (*emcmot_hal_data->pause_jog_vel < emcmotCommand[robotId]->ini_maxvel) ?
        *emcmot_hal_data->pause_jog_vel : emcmotCommand[robotId]->ini_maxvel;

        tpSetNextIdForTcId(emcmotQueue[robotId], MOTION_PAUSED_RETURN_MOVE);
        if (-1 == tpAddLine(emcmotQueue[robotId],
                           emcmotStatus[robotId]->pause_carte_pos,
                           TC_LINEAR,
                           emcmotStatus[robotId]->current_vel,
                           emcmotCommand[robotId]->ini_maxvel,
                           emcmotCommand[robotId]->acc,
                           emcmotStatus[robotId]->enables_new,
                           // FIMXE: unsure
                           0,     // FIXME: dont wait for atspeed (?)
                           -1,
                           emcmotCommand[robotId]->tag,robotId)) {
        reportError(_("can't add coordinated return move"));
        emcmotStatus[robotId]->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
        EmcCommandClass::abort_and_switchback(robotId);
        SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
        } else {
        emcmotStatus[robotId]->id = MOTION_PAUSED_RETURN_MOVE;
        SET_MOTION_ERROR_FLAG(0,emcmotStatus[robotId]->motionFlag);
        //rtapi_print_msg(RTAPI_MSG_DBG, "return move to x=%f y=%f z=%f vel=%f added\n",
//				emcmotStatus[robotId]->pause_carte_pos.tran.x,emcmotStatus[robotId]->pause_carte_pos.tran.y,
//				emcmotStatus[robotId]->pause_carte_pos.tran.z, emcmotStatus[robotId]->current_vel);
        }
        *emcmot_hal_data->pause_state = PS_RETURNING;
        break;
    }

    update_offset_pose();
    *emcmot_hal_data->pause_offset_in_range = EmcCommandClass::inRange(emcmotStatus[robotId]->pause_offset_carte_pos, 0, NULL);

    EmcPose cpos;
    tpGetPos(emcmotQueue[robotId], &cpos);

    if (*emcmot_hal_data->pause_offset_enable &&
        *emcmot_hal_data->pause_offset_in_range &&
        (!EQUAL_EMC_POSE(emcmotStatus[robotId]->pause_offset_carte_pos,cpos)) &&
        (*emcmot_hal_data->pause_jog_vel > MINIMUM_JOG_VELOCITY)) {
        // not where we should be - insert jog move.

        // clamp velocity
        emcmotStatus[robotId]->current_vel = (*emcmot_hal_data->current_vel) =
        (*emcmot_hal_data->pause_jog_vel < emcmotCommand[robotId]->ini_maxvel) ?
        *emcmot_hal_data->pause_jog_vel : emcmotCommand[robotId]->ini_maxvel;

        //rtapi_print_msg(RTAPI_MSG_DBG, "insert jog move\n");
        tpSetNextIdForTcId(emcmotQueue[robotId], MOTION_PAUSED_JOG_MOVE);
        if (-1 == tpAddLine(emcmotQueue[robotId], emcmotStatus[robotId]->pause_offset_carte_pos, TC_LINEAR,
                emcmotStatus[robotId]->current_vel , emcmotCommand[robotId]->ini_maxvel,
                emcmotCommand[robotId]->acc,
                emcmotStatus[robotId]->enables_new,  // ???
                0, // dont wait for atspeed  ???
                -1,
                emcmotCommand[robotId]->tag,robotId)) { // no indexrotary action ???
        reportError(_("can't add linear coordinated jog move"));
        emcmotStatus[robotId]->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
        EmcCommandClass::abort_and_switchback(robotId);
        SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
        } else {
        emcmotStatus[robotId]->id = MOTION_PAUSED_JOG_MOVE;
        SET_MOTION_ERROR_FLAG(0,emcmotStatus[robotId]->motionFlag);
        }
        *emcmot_hal_data->pause_state = PS_JOGGING;
    }
    break;

    case PS_JOGGING:
    if (tpIsDone(emcmotQueue[robotId])) { // jog motion stopped
        //rtapi_print_msg(RTAPI_MSG_DBG, "stopped after jog\n");

        EmcPose cpos;
        tpGetPos(emcmotQueue[robotId], &cpos);
        if (EQUAL_EMC_POSE(emcmotStatus[robotId]->pause_carte_pos,cpos)) {
        // at initial pause position.
        *emcmot_hal_data->pause_state = PS_PAUSED;
        } else {
        *emcmot_hal_data->pause_state = PS_PAUSED_IN_OFFSET;
        }
    } else {
        // jog move still in progress
        if (!*emcmot_hal_data->pause_offset_enable) {
        // stop a move in progress
        tpClear(emcmotQueue[robotId]);
        EmcPose here;
        tpGetPos(emcmotQueue[robotId], &here);
        if (EQUAL_EMC_POSE(emcmotStatus[robotId]->pause_offset_carte_pos, here)) {
            //rtapi_print_msg(RTAPI_MSG_DBG, "move stopped, in initial pause position\n");
            *emcmot_hal_data->pause_state = PS_PAUSED;
        } else {
            //rtapi_print_msg(RTAPI_MSG_DBG, "move stopped in offset\n");
            *emcmot_hal_data->pause_state = PS_PAUSED_IN_OFFSET;
        }
        }
    }
    break;

    case PS_RETURNING:
    if (tpIsDone(emcmotQueue[robotId])) { // return  motion stopped
        //rtapi_print_msg(RTAPI_MSG_DBG, "return move complete\n");
        // since resuming still active, next cycle will switch
        // back to primary q and resume
         *emcmot_hal_data->pause_state = PS_PAUSED;
    }
    break;
    }
    /* save the resulting combined scale factor */
    emcmotStatus[robotId]->net_feed_scale = scale;

    /* now do spindle scaling */
    scale = 1.0;
    if ( enables & SS_ENABLED ) {
    scale *= emcmotStatus[robotId]->spindle_scale;
    }
    /*non maskable (except during spindle synch move) spindle inhibit pin */
    if ( enables & *emcmot_hal_data->spindle_inhibit ) {
        scale = 0;
    }
    /* save the resulting combined scale factor */
    emcmotStatus[robotId]->net_spindle_scale = scale;

    /* read and process per-joint inputs */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
    /* point to joint HAL data */
    joint_data = &(emcmot_hal_data->joint[joint_num]);
    /* point to joint data */
    joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
    if (!GET_JOINT_ACTIVE_FLAG(joint)) {
        /* if joint is not active, skip it */
        continue;
    }
    /* copy data from HAL to joint structure */
    joint->index_enable = *(joint_data->index_enable);
    joint->motor_pos_fb = *(joint_data->motor_pos_fb);
    /* calculate pos_fb */
    if (( joint->home_state == HOME_INDEX_SEARCH_WAIT ) &&
        ( joint->index_enable == 0 )) {
        /* special case - we're homing the joint, and it just
           hit the index.  The encoder count might have made a
           step change.  The homing code will correct for it
           later, so we ignore motor_pos_fb and set pos_fb
           to match the commanded value instead. */
        joint->pos_fb = joint->pos_cmd;
    } else {
        /* normal case: subtract backlash comp and motor offset */
        joint->pos_fb = joint->motor_pos_fb -
        (joint->backlash_filt + joint->motor_offset);
    }
    /* calculate following error */
    joint->ferror = joint->pos_cmd - joint->pos_fb;
    abs_ferror = fabs(joint->ferror);
    /* update maximum ferror if needed */
    if (abs_ferror > joint->ferror_high_mark) {
        joint->ferror_high_mark = abs_ferror;
    }

    /* calculate following error limit */
    if (joint->vel_limit > 0.0) {
        joint->ferror_limit =
        joint->max_ferror * fabs(joint->vel_cmd) / joint->vel_limit;
    } else {
        joint->ferror_limit = 0;
    }
    if (joint->ferror_limit < joint->min_ferror) {
        joint->ferror_limit = joint->min_ferror;
    }
    /* update following error flag */
    if (abs_ferror > joint->ferror_limit) {
        SET_JOINT_FERROR_FLAG(joint, 1);
    } else {
        SET_JOINT_FERROR_FLAG(joint, 0);
    }

    /* read limit switches */
    if (*(joint_data->pos_lim_sw)) {
        SET_JOINT_PHL_FLAG(joint, 1);
    } else {
        SET_JOINT_PHL_FLAG(joint, 0);
    }
    if (*(joint_data->neg_lim_sw)) {
        SET_JOINT_NHL_FLAG(joint, 1);
    } else {
        SET_JOINT_NHL_FLAG(joint, 0);
    }
    joint->on_pos_limit = GET_JOINT_PHL_FLAG(joint);
    joint->on_neg_limit = GET_JOINT_NHL_FLAG(joint);
    /* read amp fault input */
    if (*(joint_data->amp_fault)) {
        SET_JOINT_FAULT_FLAG(joint, 1);
    } else {
        SET_JOINT_FAULT_FLAG(joint, 0);
    }

    /* read home switch input */
    if (*(joint_data->home_sw)) {
        SET_JOINT_HOME_SWITCH_FLAG(joint, 1);
    } else {
        SET_JOINT_HOME_SWITCH_FLAG(joint, 0);
    }
    /* end of read and process joint inputs loop */
    }

    // a fault was signalled during a spindle-orient in progress
    // signal error, and cancel the orient
    if (*(emcmot_hal_data->spindle_orient)) {
    if (*(emcmot_hal_data->spindle_orient_fault)) {
        emcmotStatus[robotId]->spindle.orient_state = EMCMOT_ORIENT_FAULTED;
        *(emcmot_hal_data->spindle_orient) = 0;
        emcmotStatus[robotId]->spindle.orient_fault = *(emcmot_hal_data->spindle_orient_fault);
        reportError(_("fault %d during orient in progress"), emcmotStatus[robotId]->spindle.orient_fault);
        emcmotStatus[robotId]->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
        EmcCommandClass::abort_and_switchback(robotId); // tpAbort(emcmotQueue[robotId]);
        SET_MOTION_ERROR_FLAG(1,emcmotStatus[robotId]->motionFlag);
    } else if (*(emcmot_hal_data->spindle_is_oriented)) {
        *(emcmot_hal_data->spindle_orient) = 0;
        *(emcmot_hal_data->spindle_locked) = 1;
        emcmotStatus[robotId]->spindle.locked = 1;
        emcmotStatus[robotId]->spindle.brake = 1;
        emcmotStatus[robotId]->spindle.orient_state = EMCMOT_ORIENT_COMPLETE;
        //rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked");
    }
    }
}

static void do_forward_kins(void)
{
    /* kinematics flags */
//    KINEMATICS_FORWARD_FLAGS fflags = 0;
//    KINEMATICS_INVERSE_FLAGS iflags = 0;
///* there are four possibilities for kinType:

//   IDENTITY: Both forward and inverse kins are available, and they
//   can used without an initial guess, even if one or more joints
//   are not homed.  In this case, we apply the forward kins to the
//   joint->pos_fb to produce carte_pos_fb, and if all axes are homed
//   we set carte_pos_fb_ok to 1 to indicate that the feedback data
//   is good.

//   BOTH: Both forward and inverse kins are available, but the forward
//   kins need an initial guess, and/or the kins require all joints to
//   be homed before they work properly.  Here we must tread carefully.
//   IF all the joints have been homed, we apply the forward kins to
//   the joint->pos_fb to produce carte_pos_fb, and set carte_pos_fb_ok
//   to indicate that the feedback is good.  We use the previous value
//   of carte_pos_fb as the initial guess.  If all joints have not been
//   homed, we don't call the kinematics, instead we set carte_pos_fb to
//   the cartesean coordinates of home, as stored in the global worldHome,
//   and we set carte_fb_ok to 0 to indicate that the feedback is invalid.
//\todo  FIXME - maybe setting to home isn't the right thing to do.  We need
//   it to be set to home eventually, (right before the first attemt to
//   run the kins), but that doesn't mean we should say we're at home
//   when we're not.

//   INVERSE_ONLY: Only inverse kinematics are available, forward
//   kinematics cannot be used.  So we have to fake it, the question is
//   simply "what way of faking it is best".  In free mode, or if all
//   axes have not been homed, the feedback position is unknown.  If
//   we are in teleop or coord mode, or if we are in free mode and all
//   axes are homed, and haven't been moved since they were homed, then
//   we set carte_pos_fb to carte_pos_cmd, and set carte_pos_fb_ok to 1.
//   If we are in free mode, and any joint is not homed, or any joint has
//   moved since it was homed, we leave cart_pos_fb alone, and set
//   carte_pos_fb_ok to 0.

//   FORWARD_ONLY: Only forward kinematics are available, inverse kins
//   cannot be used.  This exists for completeness only, since EMC won't
//   work without inverse kinematics.

//*/

///*! \todo FIXME FIXME FIXME - need to put a rate divider in here, run it
//   at the traj rate */

//    double joint_pos[EMCMOT_MAX_JOINTS] = {0,};
//    int joint_num, result;
//    emcmot_joint_t *joint;

//    /* copy joint position feedback to local array */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//    /* point to joint struct */
//    joint = &joints[joint_num];
//    /* copy feedback */
//    joint_pos[joint_num] = joint->pos_fb;
//    /* check for homed, only if the current joint is active */
//    if (!GET_JOINT_ACTIVE_FLAG(joint)) {
//        /* if joint is not active, don't even look at its limits */
//        continue;
//    }
//    }
//    int kinType=0;
//    switch (kinType) {

//    case KINEMATICS_IDENTITY:
//    emcmotConfig[robotId]->vtk->kinematicsForward(joint_pos, &emcmotStatus[robotId]->carte_pos_fb, &fflags,
//        &iflags);
//    if (checkAllHomed()) {
//        emcmotStatus[robotId]->carte_pos_fb_ok = 1;
//    } else {
//        emcmotStatus[robotId]->carte_pos_fb_ok = 0;
//    }
//    break;

//    case KINEMATICS_BOTH:
//    if (checkAllHomed()) {
//        /* is previous value suitable for use as initial guess? */
//        if (!emcmotStatus[robotId]->carte_pos_fb_ok) {
//        /* no, use home position as initial guess */
//        emcmotStatus[robotId]->carte_pos_fb = emcmotStatus[robotId]->world_home;
//        }
//        /* calculate Cartesean position feedback from joint pos fb */
//        result =
//        emcmotConfig[robotId]->vtk->kinematicsForward(joint_pos, &emcmotStatus[robotId]->carte_pos_fb,
//        &fflags, &iflags);
//        /* check to make sure kinematics converged */
//        if (result < 0) {
//        /* error during kinematics calculations */
//        emcmotStatus[robotId]->carte_pos_fb_ok = 0;
//        } else {
//        /* it worked! */
//        emcmotStatus[robotId]->carte_pos_fb_ok = 1;
//        }
//    } else {
//        emcmotStatus[robotId]->carte_pos_fb_ok = 0;
//    }
//    break;

//    case KINEMATICS_INVERSE_ONLY:

//    if ((GET_MOTION_COORD_FLAG()) || (GET_MOTION_TELEOP_FLAG())) {
//        /* use Cartesean position command as feedback value */
//        emcmotStatus[robotId]->carte_pos_fb = emcmotStatus[robotId]->carte_pos_cmd;
//        emcmotStatus[robotId]->carte_pos_fb_ok = 1;
//    } else {
//        emcmotStatus[robotId]->carte_pos_fb_ok = 0;
//    }
//    break;

//    default:
//    emcmotStatus[robotId]->carte_pos_fb_ok = 0;
//    break;
//    }
}

static void check_for_faults(void)
{
//    int joint_num;
//    emcmot_joint_t *joint;
//    int neg_limit_override, pos_limit_override;

//    /* check for various global fault conditions */
//    /* only check enable input if running */
//    if ( GET_MOTION_ENABLE_FLAG() != 0 ) {
//    if ( *(emcmot_hal_data->enable) == 0 ) {
//        reportError(_("motion stopped by enable input"));
//        emcmotDebug[robotId]->enabling = 0;
//    }
//    }
//    /* check for various joint fault conditions */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//    /* point to joint data */
//    joint = &joints[joint_num];
//    /* only check active, enabled axes */
//    if ( GET_JOINT_ACTIVE_FLAG(joint) && GET_JOINT_ENABLE_FLAG(joint) ) {
//        /* are any limits for this joint overridden? */
//        neg_limit_override = emcmotStatus[robotId]->overrideLimitMask & ( 1 << (joint_num*2));
//        pos_limit_override = emcmotStatus[robotId]->overrideLimitMask & ( 2 << (joint_num*2));
//        /* check for hard limits */
//        if ((GET_JOINT_PHL_FLAG(joint) && ! pos_limit_override ) ||
//        (GET_JOINT_NHL_FLAG(joint) && ! neg_limit_override )) {
//        /* joint is on limit switch, should we trip? */
//        if (GET_JOINT_HOMING_FLAG(joint)) {
//            /* no, ignore limits */
//        } else {
//            /* trip on limits */
//            if (!GET_JOINT_ERROR_FLAG(joint)) {
//            /* report the error just this once */
//            reportError(_("joint %d on limit switch error"),
//                joint_num);
//            }
//            SET_JOINT_ERROR_FLAG(joint, 1);
//            emcmotDebug[robotId]->enabling = 0;
//        }
//        }
//        /* check for amp fault */
//        if (GET_JOINT_FAULT_FLAG(joint)) {
//        /* joint is faulted, trip */
//        if (!GET_JOINT_ERROR_FLAG(joint)) {
//            /* report the error just this once */
//            reportError(_("joint %d amplifier fault"), joint_num);
//        }
//        SET_JOINT_ERROR_FLAG(joint, 1);
//        emcmotDebug[robotId]->enabling = 0;
//        }
//        /* check for excessive following error */
//        if (GET_JOINT_FERROR_FLAG(joint)) {
//        if (!GET_JOINT_ERROR_FLAG(joint)) {
//            /* report the error just this once */
//            reportError(_("joint %d following error"), joint_num);
//        }
//        SET_JOINT_ERROR_FLAG(joint, 1);
//        emcmotDebug[robotId]->enabling = 0;
//        }
//    /* end of if JOINT_ACTIVE_FLAG(joint) */
//    }
//    /* end of check for joint faults loop */
//    }
}

 void EmcControlClass::set_operating_mode(void)
{
     emcmotDebug[robotId]->coordinating=1;
     emcmotDebug[robotId]->enabling=1;
     SET_MOTION_ENABLE_FLAG(1,emcmotStatus[robotId]->motionFlag);


    int joint_num;
    emcmot_joint_t *joint;

    /* check for disabling */
    if (!emcmotDebug[robotId]->enabling && GET_MOTION_ENABLE_FLAG(emcmotStatus[robotId]->motionFlag)) {
    /* clear out the motion emcmotDebug[robotId]->queue and interpolators */
    tpClear(emcmotQueue[robotId]);
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
        /* point to joint data */
        joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
        /* disable free mode planner */
        joint->free_tp_enable = 0;
        /* drain coord mode interpolators */
        CubicClass::cubicDrain(&(joint->cubic));
        if (GET_JOINT_ACTIVE_FLAG(joint)) {
        SET_JOINT_ENABLE_FLAG(joint, 0);
        SET_JOINT_HOMING_FLAG(joint, 0);
        joint->home_state = HOME_IDLE;
        }
        /* don't clear the joint error flag, since that may signify why
           we just went into disabled state */
    }
    /* reset the trajectory interpolation counter, so that the kinematics
       functions called during the disabled state run at the nominal
       trajectory rate rather than the servo rate. It's loaded with
       emcmotConfig[robotId]->interpolationRate when it goes to zero. */
/*! \todo FIXME - interpolation is still under construction */
/*! \todo Another #if 0 */
#if 0
    interpolationCounter = 0;
#endif
    SET_MOTION_ENABLE_FLAG(0,emcmotStatus[robotId]->motionFlag);
    /* don't clear the motion error flag, since that may signify why we
       just went into disabled state */
    }

    /* check for emcmotDebug[robotId]->enabling */
    if (emcmotDebug[robotId]->enabling && !GET_MOTION_ENABLE_FLAG(emcmotStatus[robotId]->motionFlag)) {
    tpSetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd);
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
        /* point to joint data */
        joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];

        joint->free_pos_cmd = joint->pos_cmd;
        if (GET_JOINT_ACTIVE_FLAG(joint)) {
        SET_JOINT_ENABLE_FLAG(joint, 1);
        SET_JOINT_HOMING_FLAG(joint, 0);
        joint->home_state = HOME_IDLE;
        }
        /* clear any outstanding joint errors when going into enabled
           state */
        SET_JOINT_ERROR_FLAG(joint, 0);
    }
    SET_MOTION_ENABLE_FLAG(1,emcmotStatus[robotId]->motionFlag);
    /* clear any outstanding motion errors when going into enabled state */
    SET_MOTION_ERROR_FLAG(0,emcmotStatus[robotId]->motionFlag);
    }

    /* check for entering teleop mode */
//    if (emcmotDebug[robotId]->teleoperating && !GET_MOTION_TELEOP_FLAG()) {
//    if (GET_MOTION_INPOS_FLAG()) {

//        /* update coordinated emcmotDebug[robotId]->queue position */
//        tpSetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd);

//        /* drain the cubics so they'll synch up */
//        for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint data */
//        joint = &joints[joint_num];
//        CubicClass::cubicDrain(&(joint->cubic));
//        }
//        /* Initialize things to do when starting teleop mode. */
//        ZERO_EMC_POSE(emcmotDebug[robotId]->teleop_data.currentVel);
//        ZERO_EMC_POSE(emcmotDebug[robotId]->teleop_data.desiredVel);
//        ZERO_EMC_POSE(emcmotDebug[robotId]->teleop_data.currentAccel);
//        ZERO_EMC_POSE(emcmotDebug[robotId]->teleop_data.desiredAccel);
//        SET_MOTION_TELEOP_FLAG(1);
//        SET_MOTION_ERROR_FLAG(0);
//    } else {
//        /* not in position-- don't honor mode change */
//        emcmotDebug[robotId]->teleoperating = 0;
//    }
//    } else {
//    if (GET_MOTION_INPOS_FLAG()) {
//        if (!emcmotDebug[robotId]->teleoperating && GET_MOTION_TELEOP_FLAG()) {
//        SET_MOTION_TELEOP_FLAG(0);
//        if (!emcmotDebug[robotId]->coordinating) {
//            for (joint_num = 0; joint_num < num_joints;
//            joint_num++) {
//            /* point to joint data */
//            joint = &joints[joint_num];
//            /* update free planner positions */
//            joint->free_pos_cmd = joint->pos_cmd;
//            }
//        }
//        }
//    }

    /* check for entering coordinated mode */
    if (emcmotDebug[robotId]->coordinating && !GET_MOTION_COORD_FLAG(emcmotStatus[robotId]->motionFlag)) {
        if (GET_MOTION_INPOS_FLAG(emcmotStatus[robotId]->motionFlag)) {
        /* preset traj planner to current position */
        //
        tpSetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd);

        /* drain the cubics so they'll synch up */
        for (joint_num = 0; joint_num < num_joints; joint_num++) {
            /* point to joint data */
            joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
            CubicClass::cubicDrain(&(joint->cubic));
        }
        /* clear the override limits flags */
        emcmotDebug[robotId]->overriding = 0;
        emcmotStatus[robotId]->overrideLimitMask = 0;
        SET_MOTION_COORD_FLAG(1,emcmotStatus[robotId]->motionFlag);
        SET_MOTION_TELEOP_FLAG(0,emcmotStatus[robotId]->motionFlag);
        SET_MOTION_ERROR_FLAG(0,emcmotStatus[robotId]->motionFlag);
        } else {
        /* not in position-- don't honor mode change */
        emcmotDebug[robotId]->coordinating = 0;
        }
    }

//    /* check entering free space mode */
//    if (!emcmotDebug[robotId]->coordinating && GET_MOTION_COORD_FLAG()) {
//        if (GET_MOTION_INPOS_FLAG()) {
//        for (joint_num = 0; joint_num < num_joints; joint_num++) {
//            /* point to joint data */
//            joint = &joints[joint_num];
//            /* set joint planner pos cmd to current location */
//            joint->free_pos_cmd = joint->pos_cmd;
//            /* but it can stay disabled until a move is required */
//            joint->free_tp_enable = 0;
//        }
//        SET_MOTION_COORD_FLAG(0);
//        SET_MOTION_TELEOP_FLAG(0);
//        SET_MOTION_ERROR_FLAG(0);
//        } else {
//        /* not in position-- don't honor mode change */
//        emcmotDebug[robotId]->coordinating = 1;
//        }
//    }
//    }
//    /*! \todo FIXME - this code is temporary - eventually this function will be
//       cleaned up and simplified, and 'motion_state' will become the master
//       for this info, instead of having to gather it from several flags */
    if (!GET_MOTION_ENABLE_FLAG(emcmotStatus[robotId]->motionFlag)) {
    emcmotStatus[robotId]->motion_state = EMCMOT_MOTION_DISABLED;
    } else if (GET_MOTION_TELEOP_FLAG(emcmotStatus[robotId]->motionFlag)) {
    emcmotStatus[robotId]->motion_state = EMCMOT_MOTION_TELEOP;
    } else if (GET_MOTION_COORD_FLAG(emcmotStatus[robotId]->motionFlag)) {
    emcmotStatus[robotId]->motion_state = EMCMOT_MOTION_COORD;
    } else {
    emcmotStatus[robotId]->motion_state = EMCMOT_MOTION_FREE;
    }
 }

 void EmcControlClass::resetJointsCommand(std::vector<double> &jointsIn)
 {
     emcmot_joint_t *joint;
     for (int joint_num = 0; joint_num < num_joints; joint_num++)
     {
         if(joint_num<jointsIn.size())
         {
             joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
             joint->coarse_pos = jointsIn[joint_num];
             joint->pos_cmd = jointsIn[joint_num];
             joint->simpleTp.curr_pos=joint->pos_cmd;
             joint->simpleTp.curr_vel=0;
             joint->simpleTp.pos_cmd=joint->pos_cmd;
             joint->simpleTp.enable=0;

         }

     }
 }

 int EmcControlClass::getJointCommand(std::vector<double> &jointCommandOut)
 {
     jointCommandOut.resize(num_joints);
     emcmot_joint_t *joint;
     for (int joint_num = 0; joint_num < num_joints; joint_num++)
     {
         joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
         jointCommandOut[joint_num]=joint->pos_cmd;

     }
     return 1;

 }

 int EmcControlClass::setNetFeedScale(double scaleIn)
 {
     emcmotStatus[robotId]->net_feed_scale=scaleIn;
     return 1;
 }

 double EmcControlClass::getNetFeedScale()
 {
     return emcmotStatus[robotId]->net_feed_scale;
 }

 E_WAVE_STATUS EmcControlClass::getWaveMotionStatus()
 {
     return waveMotionControl->getWaveMotionStatus();
 }

 int EmcControlClass::setTimeAxisScale(double ratio)
 {
     return 1;
//     return waveMotionControl->setTimeAxisScale(ratio);
 }

 int EmcControlClass::startWave()
 {
    return waveMotionControl->startWave();
 }

 int EmcControlClass::stopWave()
 {
     return waveMotionControl->stopWave();
 }

 int EmcControlClass::ceaseWave()
 {
     return waveMotionControl->ceaseWave();
 }

 int EmcControlClass::setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn,
                                  double wavePeriodIn)
 {
     return waveMotionControl->setWaveType(typeIn,waveWidthIn,wavePeriodIn);
 }

 int EmcControlClass::setWaveMotionInfo(WeaveMotionCondition infoIn)
 {
     return waveMotionControl->setWaveMotionInfo(infoIn);
 }

 int EmcControlClass::setWaveAcceleration(double accelerationIn,double maxVelIn,bool isRatio)
 {
     return waveMotionControl->setWaveAcceleration(accelerationIn,maxVelIn,isRatio);
 }

 int EmcControlClass::get_pos_cmds_coordinateRescale(long period)
 {
     int joint_num, result;
     double positions[EMCMOT_MAX_JOINTS];
     emcmot_joint_t *joint;
     double old_pos_cmd;
     /* 直线取点，并填充关节目标点 */
     /* they're empty, pull next point(s) off Cartesian planner */
     /* run coordinated trajectory planning cycle */
     tpRunCycle(emcmotQueue[robotId], period,robotId);

     static int tickCount=0;
     /* gt new commanded traj pos */
     if(TP_ERR_OK==tpGetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd))
     {
//            printEmcPose(&emcmotStatus[robotId]->carte_pos_cmd);
//            printPose(emcmotStatus[robotId]->carte_pos_cmd);
         tickCount++;
//            std::cout<<"tickCount"<<tickCount<<std::endl;

     }

     std::vector<double>  initialJointIn;
     std::vector<double> posIn,posOut,jointOut;
     posIn.resize(6);
     posIn[0]=emcmotStatus[robotId]->carte_pos_cmd.tran.x;
     posIn[1]=emcmotStatus[robotId]->carte_pos_cmd.tran.y;
     posIn[2]=emcmotStatus[robotId]->carte_pos_cmd.tran.z;
     posIn[3]=emcmotStatus[robotId]->carte_pos_cmd.a;
     posIn[4]=emcmotStatus[robotId]->carte_pos_cmd.b;
     posIn[5]=emcmotStatus[robotId]->carte_pos_cmd.c;
     initialJointIn.resize(num_joints);
     for (joint_num = 0; joint_num < num_joints; joint_num++)
     {
         initialJointIn[joint_num] = joints[joint_num+robotId*EMCMOT_MAX_JOINTS].pos_cmd;
     }
//                std::cout<<"posIn"<<posIn[0]<<posIn[1]<<posIn[2]
//                          <<posIn[3]<<posIn[4]<<posIn[5]<<std::endl;
     //叠加摆动
     waveMotionControl->moveOneStep(tpGetCurrentVelReachRatio(emcmotQueue[robotId]),posIn,posOut);
//    std::cout<<"pos error"<<posOut[0]-posIn[0]<<" "<<posOut[1]-posIn[1]<<" "<<posOut[2]-posIn[2]<<std::endl;
     if(1==cavingModule->kinematicsInverse(posOut, initialJointIn,jointOut,true))
     {
         innerAxisNum=jointOut.size();
         /* copy to joint structures and spline them up */
         for (joint_num = 0; joint_num < num_joints; joint_num++)
         {
             if(joint_num<jointOut.size())
             {
                 joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
                 joint->coarse_pos = jointOut[joint_num];

             }

         }


     }
     else
     {
         //tpPause();
         result= -100;
         return result;
     }




     /* 直线关节插值 */
     for (joint_num = 0; joint_num < num_joints; joint_num++)
     {
         /* point to joint struct */
         joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
         /* save old command */
         old_pos_cmd = joint->pos_cmd;
         /* interpolate to get new one */
         joint->pos_cmd =joint->coarse_pos;

         joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;



     }
     //重新调节整体速度.因为收到线加速度的约束。可能在奇异点附近速度突变特别大的情况，无法及时降速，导致超过关节速度限制。
     double tmpVelRatio=getJointVelOverRatio();
     if(tmpVelRatio>1)
     {
//         setNetFeedScale(tmpNewScale);
         tpRescaleVelLimitByCurrentVel(emcmotQueue[robotId],1.0/tmpVelRatio);
     }
     else
     {
         tpRestoreVelLimitByCurrentVel(emcmotQueue[robotId]);
     }
     g_waveMonitorData[0]=tmpVelRatio;
     //todo 速度恢复
//     g_waveMonitorData[1]=getNetFeedScale();
//     std::cout<<"/////////////tmpVelRatio"<<g_waveMonitorData[0]<<"NetFeedScale"<<g_waveMonitorData[1]<<std::endl;

     /* report motion status */
     SET_MOTION_INPOS_FLAG(0,emcmotStatus[robotId]->motionFlag);
     if (tpIsDone(emcmotQueue[robotId]))
     {
         SET_MOTION_INPOS_FLAG(1,emcmotStatus[robotId]->motionFlag);
     }
     return result;
 }

 int EmcControlClass::get_pos_cmds_coordinateSimpleTp(long period)
 {

 }



void EmcControlClass::handle_jogwheels(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    joint_hal_t *joint_data;
    int new_jog_counts, delta;
    double distance, pos, stop_dist;

    for (joint_num = 0; joint_num < num_joints; joint_num++) {
    /* point to joint data */
    joint_data = &(emcmot_hal_data->joint[joint_num]);
    joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
    if (!GET_JOINT_ACTIVE_FLAG(joint)) {
        /* if joint is not active, skip it */
        continue;
    }
    /* get counts from jogwheel */
    new_jog_counts = *(joint_data->jog_counts);
    delta = new_jog_counts - joint->old_jog_counts;
    /* save value for next time */
    joint->old_jog_counts = new_jog_counts;
    /* initialization complete */
//    if ( first_pass ) {
//        continue;
//    }
    /* did the wheel move? */
    if ( delta == 0 ) {
        /* no, nothing to do */
        continue;
    }
    /* must be in free mode and enabled */
    if (GET_MOTION_COORD_FLAG(emcmotStatus[robotId]->motionFlag)) {
        continue;
    }
    if (!GET_MOTION_ENABLE_FLAG(emcmotStatus[robotId]->motionFlag)) {
        continue;
    }
    /* the jogwheel input for this joint must be enabled */
    if ( *(joint_data->jog_enable) == 0 ) {
        continue;
    }
    /* must not be homing */
    if (emcmotStatus[robotId]->homing_active) {
        continue;
    }
    /* must not be doing a keyboard jog */
    if (joint->kb_jog_active) {
        continue;
    }
    if (emcmotStatus[robotId]->net_feed_scale < 0.0001 ) {
        /* don't jog if feedhold is on or if feed override is zero */
        break;
    }
    /* calculate distance to jog */
    distance = delta * *(joint_data->jog_scale);
    /* check for joint already on hard limit */
    if (distance > 0.0 && GET_JOINT_PHL_FLAG(joint)) {
        continue;
    }
    if (distance < 0.0 && GET_JOINT_NHL_FLAG(joint)) {
        continue;
    }
    /* calc target position for jog */
    pos = joint->free_pos_cmd + distance;
    /* don't jog past limits */
    refresh_jog_limits(joint);
    if (pos > joint->max_jog_limit) {
        continue;
    }
    if (pos < joint->min_jog_limit) {
        continue;
    }
    /* The default is to move exactly as far as the wheel commands,
       even if that move takes much longer than the wheel movement
       that commanded it.  Some folks prefer that the move stop as
       soon as the wheel does, even if that means not moving the
       commanded distance.  Velocity mode is for those folks.  If
       the command is faster than the machine can track, excess
       command is simply dropped. */
    if ( *(joint_data->jog_vel_mode) ) {
            double v = joint->vel_limit * emcmotStatus[robotId]->net_feed_scale;
        /* compute stopping distance at max speed */
        stop_dist = v * v / ( 2 * joint->acc_limit);
        /* if commanded position leads the actual position by more
           than stopping distance, discard excess command */
        if ( pos > joint->pos_cmd + stop_dist ) {
        pos = joint->pos_cmd + stop_dist;
        } else if ( pos < joint->pos_cmd - stop_dist ) {
        pos = joint->pos_cmd - stop_dist;
        }
    }
        /* set target position and use full velocity */
        joint->free_pos_cmd = pos;
        joint->free_vel_lim = joint->vel_limit;
    /* lock out other jog sources */
    joint->wheel_jog_active = 1;
        /* and let it go */
        joint->free_tp_enable = 1;
    SET_JOINT_ERROR_FLAG(joint, 0);
    /* clear joint homed flag(s) if we don't have forward kins.
       Otherwise, a transition into coordinated mode will incorrectly
       assume the homed position. Do all if they've all been moved
       since homing, otherwise just do this one */
    clearHomes(joint_num);
    }
}

int EmcControlClass::get_pos_cmds_coordinateCubic(long period)
{
    int joint_num, result;
    double positions[EMCMOT_MAX_JOINTS];
    emcmot_joint_t *joint;
    double old_pos_cmd;
    while (CubicClass::cubicNeedNextPoint(&(joints[0+robotId*EMCMOT_MAX_JOINTS].cubic)))
    {
        switch (interpoStatus) {
        case D_INTERPOLSTION_STATUS_NORMAL:
        {

                /* they're empty, pull next point(s) off Cartesian planner */
                /* run coordinated trajectory planning cycle */
                tpRunCycle(emcmotQueue[robotId], period,robotId);

                static int tickCount=0;
                /* gt new commanded traj pos */
                if(TP_ERR_OK==tpGetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd))
                {
        //            printEmcPose(&emcmotStatus[robotId]->carte_pos_cmd);
        //            printPose(emcmotStatus[robotId]->carte_pos_cmd);
                    tickCount++;
        //            std::cout<<"tickCount"<<tickCount<<std::endl;

                }

                std::vector<double>  initialJointIn;
                std::vector<double> posIn,jointOut;
                posIn.resize(6);
                posIn[0]=emcmotStatus[robotId]->carte_pos_cmd.tran.x;
                posIn[1]=emcmotStatus[robotId]->carte_pos_cmd.tran.y;
                posIn[2]=emcmotStatus[robotId]->carte_pos_cmd.tran.z;
                posIn[3]=emcmotStatus[robotId]->carte_pos_cmd.a;
                posIn[4]=emcmotStatus[robotId]->carte_pos_cmd.b;
                posIn[5]=emcmotStatus[robotId]->carte_pos_cmd.c;
                initialJointIn.resize(num_joints);
                for (joint_num = 0; joint_num < num_joints; joint_num++)
                {
                    initialJointIn[joint_num] = joints[joint_num+robotId*EMCMOT_MAX_JOINTS].pos_cmd;
                }
//                std::cout<<"posIn"<<posIn[0]<<posIn[1]<<posIn[2]
//                          <<posIn[3]<<posIn[4]<<posIn[5]<<std::endl;
                if(1==cavingModule->kinematicsInverse(posIn, initialJointIn,jointOut,true))
                {
                    innerAxisNum=jointOut.size();
                    /* copy to joint structures and spline them up */
                    //判断新点插入后速度是否超过关节限速，如果超过，则插入限速关节位置，不再取新的点。直到限速恢复正常。
                    bool isVelOk=true;
                    for (joint_num = 0; joint_num < num_joints; joint_num++)
                    {
                        if(joint_num<jointOut.size())
                        {
                            joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
                            joint->coarse_pos = jointOut[joint_num];
                            int velCheckResult=isJointVelPlanOk(joint_num);
                            if(0==velCheckResult)
                            {

                                CubicClass::cubicAddPoint(&(joint->cubic), joint->coarse_pos);
//                                std::cout<<"interpoStatus"<<interpoStatus<<"joint_num"<<joint_num
//                                          <<"add coarse_pos"<<joint->coarse_pos<<std::endl;
                                g_waveMonitorData[joint_num]=0;
                            }
                            else
                            {
                                isVelOk=false;
//                                double deltaPos=velCheckResult*joint->vel_limit*emcmotConfig[robotId]->trajCycleTime*arm_warnSlowRatio;
//                                CubicClass::cubicAddVelFilterPoint(&(joint->cubic), deltaPos);
                                CubicClass::cubicAddVelLimitPoint(&(joint->cubic),joint->vel_limit*arm_warnSlowRatio
                                                                  ,joint->acc_limit,velCheckResult);
//                                std::cout<<"interpoStatus"<<interpoStatus<<"joint_num"<<joint_num
//                                          <<"add filtDeltaPos"<<deltaPos<<std::endl;
//                                double newMiddlePos=joint->pos_cmd+deltaPos;
//                                CubicClass::cubicAddPoint(&(joint->cubic), newMiddlePos);
                                g_waveMonitorData[joint_num]=100;
                            }
                        }

                    }
                    if(false==isVelOk)
                    {

                        interpoStatus=D_INTERPOLSTION_STATUS_SPEEDLIMIT;

                    }


                }
                else
                {
                    //tpPause();
                    result= -100;
                    return result;
                }

        }
            break;
        case D_INTERPOLSTION_STATUS_SPEEDLIMIT:
        {

            bool isVelOk=true;
            for (joint_num = 0; joint_num < num_joints; joint_num++)
            {
                if(joint_num<innerAxisNum)
                {
                    joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
                    int velCheckResult=isJointVelPlanOk(joint_num);
                    if(0==velCheckResult)
                    {
                        //突然给相同的目标值，导致加速度冲击太大了。
                        CubicClass::cubicAddPoint(&(joint->cubic), joint->coarse_pos);
//                        std::cout<<"interpoStatus"<<interpoStatus<<" joint_num"<<joint_num
//                               <<" add coarse_pos"<<joint->coarse_pos<<std::endl;
                        g_waveMonitorData[joint_num]=0;
                    }
                    else
                    {
                        isVelOk=false;
//                        double deltaPos=velCheckResult*joint->vel_limit*emcmotConfig[robotId]->trajCycleTime*arm_warnSlowRatio;
//                        CubicClass::cubicAddVelFilterPoint(&(joint->cubic), deltaPos);
                        //限制速度与恢复切换，导致加速度冲击太大了。
                        CubicClass::cubicAddVelLimitPoint(&(joint->cubic),joint->vel_limit*arm_warnSlowRatio
                                                          ,joint->acc_limit,velCheckResult);
//                        std::cout<<"interpoStatus"<<interpoStatus<<" joint_num"<<joint_num
//                               <<" add filtDeltaPos"<<deltaPos<<std::endl;
//                        double newMiddlePos=joint->pos_cmd+deltaPos;
//                        CubicClass::cubicAddPoint(&(joint->cubic), newMiddlePos);
                        g_waveMonitorData[joint_num]=100;
                    }
                }

            }
            if(true==isVelOk)
            {

                interpoStatus=D_INTERPOLSTION_STATUS_NORMAL;

            }
        }
            break;
        default:
            break;
        }
    }



    /* there is data in the interpolators */
    /* run interpolation */
    for (joint_num = 0; joint_num < num_joints; joint_num++)
    {
        /* point to joint struct */
        joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
        /* save old command */
        old_pos_cmd = joint->pos_cmd;
        /* interpolate to get new one */
        double tmpV,tmpA,tmpJ;
//        joint->pos_cmd = CubicClass::cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
        joint->pos_cmd = CubicClass::cubicInterpolate(&(joint->cubic), 0, &tmpV, &tmpA, &tmpJ);
        g_waveMonitorData[6+joint_num]=tmpA;
        joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
    }
    /* report motion status */
    SET_MOTION_INPOS_FLAG(0,emcmotStatus[robotId]->motionFlag);
    if (tpIsDone(emcmotQueue[robotId]))
    {
        SET_MOTION_INPOS_FLAG(1,emcmotStatus[robotId]->motionFlag);
    }
    return result;
}


int EmcControlClass::get_pos_cmds_coordinate(long period)
{
    /* check joint 0 to see if the interpolators are empty */
//    while (CubicClass::cubicNeedNextPoint(&(joints[0].cubic)))
//    {
        /* they're empty, pull next point(s) off Cartesian planner */
        /* run coordinated trajectory planning cycle */
        tpRunCycle(emcmotQueue[robotId], period,robotId);

        static int tickCount=0;
        /* gt new commanded traj pos */
        if(TP_ERR_OK==tpGetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd))
        {
//            printEmcPose(&emcmotStatus[robotId]->carte_pos_cmd);
//            printPose(emcmotStatus[robotId]->carte_pos_cmd);
//            tickCount++;
//            std::cout<<"tickCount"<<tickCount<<std::endl;

        }


        /* OUTPUT KINEMATICS - convert to joints in local array */
//        emcmotConfig[robotId]->vtk->kinematicsInverse(&emcmotStatus[robotId]->carte_pos_cmd, positions,
//                         &iflags, &fflags);
        /* copy to joint structures and spline them up */
//        for (joint_num = 0; joint_num < num_joints; joint_num++)
//        {
//            /* point to joint struct */
//            joint = &joints[joint_num];
//            joint->coarse_pos = positions[joint_num];
//            /* spline joints up-- note that we may be adding points
//               that fail soft limits, but we'll abort at the end of
//               this cycle so it doesn't really matter */
//            CubicClass::cubicAddPoint(&(joint->cubic), joint->coarse_pos);
//        }
//        /* END OF OUTPUT KINS */
//    }
    /* there is data in the interpolators */
    /* run interpolation */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        /* save old command */
//        old_pos_cmd = joint->pos_cmd;
//        /* interpolate to get new one */
//        joint->pos_cmd = CubicClass::cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
//        joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
//    }
    /* report motion status */
    SET_MOTION_INPOS_FLAG(0,emcmotStatus[robotId]->motionFlag);
    if (tpIsDone(emcmotQueue[robotId]))
    {
        SET_MOTION_INPOS_FLAG(1,emcmotStatus[robotId]->motionFlag);
    }
    return 1;
}

int EmcControlClass::isJointVelPlanOk(int jointIndex)
{
    emcmot_joint_t *joint;
    joint = &joints[jointIndex+robotId*EMCMOT_MAX_JOINTS];
    double forcaseVel;
    int tmpKey=CubicClass::cubicCheckForcaseVelLimit(&joint->cubic,joint->coarse_pos,forcaseVel);
    if(1==tmpKey)
    {
        return 0;
    }
    if(joint->vel_limit<forcaseVel)
    {
        return 1;
    }
    else if(-joint->vel_limit>forcaseVel)
    {
        return -1;
    }
    return 0;

}

int EmcControlClass::isAllJointReachedTarged()
{

}

double EmcControlClass::getJointVelOverRatio()
{
    emcmot_joint_t *joint;
    double maxRatio=0;
    for (int joint_num = 0; joint_num < num_joints; joint_num++)
    {
        joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
        double tmpRatio=fabs(joint->vel_cmd/joint->vel_limit);
        if(tmpRatio>maxRatio)
        {
            maxRatio=tmpRatio;
//            if(tmpRatio>=1)
//            {
//                std::cout<<"~~~~~~~~error,,,getJointVelOverRatio tmpRatio"<<tmpRatio<<" joint_num"<<joint_num
//                       <<" vel_cmd"<<joint->vel_cmd<<" vel_limit"<<joint->vel_limit<<" robotId"<<robotId
//                      <<" acc_limit"<<joint->acc_limit<<" f "<<joints[joint_num+robotId*EMCMOT_MAX_JOINTS+1].vel_limit
//                     <<" f "<<joints[joint_num+robotId*EMCMOT_MAX_JOINTS+2].vel_limit
//                    <<" f "<<joints[joint_num+robotId*EMCMOT_MAX_JOINTS+3].vel_limit
//                   <<" f "<<joints[joint_num+robotId*EMCMOT_MAX_JOINTS+4].vel_limit<<std::endl;
//            }

        }

    }
    return maxRatio;
}

void EmcControlClass::get_pos_cmds(long period)
{
//    /* kinematics flags */
//    KINEMATICS_FORWARD_FLAGS fflags = 0;
//    KINEMATICS_INVERSE_FLAGS iflags = 0;
//    int kinType=0;
//    int joint_num, result;
//    emcmot_joint_t *joint;
//    double positions[EMCMOT_MAX_JOINTS];
///*! \todo Another #if 0 */
//#if 0
//    static int interpolationCounter = 0;
//#endif
//    double old_pos_cmd;
//    double max_dv, tiny_dp, pos_err, vel_req, vel_lim;

//    /* used in teleop mode to compute the max accell requested */
//    double accell_mag;
//    int onlimit;

//    /* copy joint position feedback to local array */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//    /* point to joint struct */
//    joint = &joints[joint_num];
//    /* copy coarse command */
//    positions[joint_num] = joint->coarse_pos;
//    }
//    /* if less than a full complement of joints, zero out the rest */
//    while ( joint_num < EMCMOT_MAX_JOINTS ) {
//        positions[joint_num++] = 0.0;
//    }


//    /* RUN MOTION CALCULATIONS: */

//    /* run traj planner code depending on the state */
//    switch ( emcmotStatus[robotId]->motion_state) {
//    case EMCMOT_MOTION_FREE:
//    /* in free mode, each joint is planned independently */
//    /* Each joint has a very simple "trajectory planner".  If the planner
//       is enabled (free_tp_enable), then it moves toward free_pos_cmd at
//       free_vel_lim, obeying the joint's accel and velocity limits, and
//       stopping when it gets there.  If it is not enabled, it stops as
//       quickly as possible, again obeying the accel limit.  When
//       disabled, free_pos_cmd is set to the current position. */
//    /* initial value for flag, if needed it will be cleared below */
//    SET_MOTION_INPOS_FLAG(1);
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        if(joint->acc_limit > emcmotStatus[robotId]->acc)
//        joint->acc_limit = emcmotStatus[robotId]->acc;
//        //AJ: only need to worry about free mode if joint is active
//        if (GET_JOINT_ACTIVE_FLAG(joint)) {
//        joint->free_tp_active = 0;
//        /* compute max change in velocity per servo period */
//        max_dv = joint->acc_limit * servo_period;
//        /* compute a tiny position range, to be treated as zero */
//        tiny_dp = max_dv * servo_period * 0.001;
//        /* calculate desired velocity */
//        if (joint->free_tp_enable) {
//            /* planner enabled, request a velocity that tends to drive
//               pos_err to zero, but allows for stopping without position
//               overshoot */
//            pos_err = joint->free_pos_cmd - joint->pos_cmd;
//            /* positive and negative errors require some sign flipping to
//               avoid sqrt(negative) */
//            if (pos_err > tiny_dp) {
//            vel_req =
//                -max_dv + sqrt(2.0 * joint->acc_limit * pos_err +
//                max_dv * max_dv);
//            /* mark joint active */
//            joint->free_tp_active = 1;
//            } else if (pos_err < -tiny_dp) {
//            vel_req =
//                max_dv - sqrt(-2.0 * joint->acc_limit * pos_err +
//                max_dv * max_dv);
//            /* mark joint active */
//            joint->free_tp_active = 1;
//            } else {
//            /* within 'tiny_dp' of desired pos, no need to move */
//            vel_req = 0.0;
//            }
//        } else {
//            /* planner disabled, request zero velocity */
//            vel_req = 0.0;
//            /* and set command to present position to avoid movement when
//               next enabled */
//            joint->free_pos_cmd = joint->pos_cmd;
//        }
//        /* if we move at all, clear AT_HOME flag */
//        if (joint->free_tp_active) {
//            SET_JOINT_AT_HOME_FLAG(joint, 0);
//        }
//                if ( joint->home_state == HOME_IDLE ) {
//                    /* velocity limit = planner limit * global scale factor */
//                    /* the global factor is used for feedrate override */
//                    vel_lim = joint->free_vel_lim * emcmotStatus[robotId]->net_feed_scale;
//                } else {
//                    /* except if homing, when we ignore FO */
//                    vel_lim = joint->free_vel_lim;
//                }
//        /* must not be greater than the joint physical limit */
//        if (vel_lim > joint->vel_limit) {
//            vel_lim = joint->vel_limit;
//        }
//        /* limit velocity request */
//        if (vel_req > vel_lim) {
//            vel_req = vel_lim;
//        } else if (vel_req < -vel_lim) {
//            vel_req = -vel_lim;
//        }
//        /* ramp velocity toward request at joint accel limit */
//        if (vel_req > joint->vel_cmd + max_dv) {
//            joint->vel_cmd += max_dv;
//        } else if (vel_req < joint->vel_cmd - max_dv) {
//            joint->vel_cmd -= max_dv;
//        } else {
//            joint->vel_cmd = vel_req;
//        }
//        /* check for still moving */
//        if (joint->vel_cmd != 0.0) {
//            /* yes, mark joint active */
//            joint->free_tp_active = 1;
//        }
//        /* integrate velocity to get new position */
//        joint->pos_cmd += joint->vel_cmd * servo_period;
//        /* copy to coarse_pos */
//        joint->coarse_pos = joint->pos_cmd;
//        /* update joint status flag and overall status flag */
//        if ( joint->free_tp_active ) {
//            /* active TP means we're moving, so not in position */
//            SET_JOINT_INPOS_FLAG(joint, 0);
//            SET_MOTION_INPOS_FLAG(0);
//            /* is any limit disabled for this move? */
//            if ( emcmotStatus[robotId]->overrideLimitMask ) {
//            emcmotDebug[robotId]->overriding = 1;
//            }
//        } else {
//            SET_JOINT_INPOS_FLAG(joint, 1);
//            /* joint has stopped, so any outstanding jogs are done */
//            joint->kb_jog_active = 0;
//            joint->wheel_jog_active = 0;
//        }
//        }//if (GET_JOINT_ACTIVE_FLAG(join))
//    }//for loop for joints
//    /* if overriding is true and we're in position, the jog
//       is complete, and the limits should be re-enabled */
//    if ( (emcmotDebug[robotId]->overriding ) && ( GET_MOTION_INPOS_FLAG() ) ) {
//        emcmotStatus[robotId]->overrideLimitMask = 0;
//        emcmotDebug[robotId]->overriding = 0;
//    }
//    /*! \todo FIXME - this should run at the traj rate */

//    switch (kinType) {

//    case KINEMATICS_IDENTITY:
//        emcmotConfig[robotId]->vtk->kinematicsForward(positions, &emcmotStatus[robotId]->carte_pos_cmd, &fflags, &iflags);
//        if (checkAllHomed()) {
//        emcmotStatus[robotId]->carte_pos_cmd_ok = 1;
//        } else {
//        emcmotStatus[robotId]->carte_pos_cmd_ok = 0;
//        }
//        break;

//    case KINEMATICS_BOTH:
//        if (checkAllHomed()) {
//        /* is previous value suitable for use as initial guess? */
//        if (!emcmotStatus[robotId]->carte_pos_cmd_ok) {
//            /* no, use home position as initial guess */
//            emcmotStatus[robotId]->carte_pos_cmd = emcmotStatus[robotId]->world_home;
//        }
//        /* calculate Cartesean position command from joint coarse pos cmd */
//        result =
//            emcmotConfig[robotId]->vtk->kinematicsForward(positions, &emcmotStatus[robotId]->carte_pos_cmd, &fflags, &iflags);
//        /* check to make sure kinematics converged */
//        if (result < 0) {
//            /* error during kinematics calculations */
//            emcmotStatus[robotId]->carte_pos_cmd_ok = 0;
//        } else {
//            /* it worked! */
//            emcmotStatus[robotId]->carte_pos_cmd_ok = 1;
//        }
//        } else {
//        emcmotStatus[robotId]->carte_pos_cmd_ok = 0;
//        }
//        break;

//    case KINEMATICS_INVERSE_ONLY:
//        emcmotStatus[robotId]->carte_pos_cmd_ok = 0;
//        break;

//    default:
//        emcmotStatus[robotId]->carte_pos_cmd_ok = 0;
//        break;
//    }
//        /* end of FREE mode */
//    break;
//    case EMCMOT_MOTION_COORD:
//    /* check joint 0 to see if the interpolators are empty */
//    while (CubicClass::cubicNeedNextPoint(&(joints[0].cubic))) {
//        /* they're empty, pull next point(s) off Cartesian planner */
//        /* run coordinated trajectory planning cycle */
//        tpRunCycle(emcmotQueue[robotId], period,robotId);

//        /* gt new commanded traj pos */
//        tpGetPos(emcmotQueue[robotId], &emcmotStatus[robotId]->carte_pos_cmd);

//        /* OUTPUT KINEMATICS - convert to joints in local array */
//        emcmotConfig[robotId]->vtk->kinematicsInverse(&emcmotStatus[robotId]->carte_pos_cmd, positions,
//                         &iflags, &fflags);
//        /* copy to joint structures and spline them up */
//        for (joint_num = 0; joint_num < num_joints; joint_num++)
//        {
//            /* point to joint struct */
//            joint = &joints[joint_num];
//            joint->coarse_pos = positions[joint_num];
//            /* spline joints up-- note that we may be adding points
//               that fail soft limits, but we'll abort at the end of
//               this cycle so it doesn't really matter */
//            CubicClass::cubicAddPoint(&(joint->cubic), joint->coarse_pos);
//        }
//        /* END OF OUTPUT KINS */
//    }
//    /* there is data in the interpolators */
//    /* run interpolation */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        /* save old command */
//        old_pos_cmd = joint->pos_cmd;
//        /* interpolate to get new one */
//        joint->pos_cmd = CubicClass::cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
//        joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
//    }
//    /* report motion status */
//    SET_MOTION_INPOS_FLAG(0);
//    if (tpIsDone(emcmotQueue[robotId])) {
//        SET_MOTION_INPOS_FLAG(1);
//    }
//    break;
//    case EMCMOT_MOTION_TELEOP:

//    /* first the desired Accel's are computed based on
//        desired Velocity, current velocity and period */
//    emcmotDebug[robotId]->teleop_data.desiredAccel.tran.x =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.tran.x -
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.x) /
//        servo_period;
//    emcmotDebug[robotId]->teleop_data.desiredAccel.tran.y =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.tran.y -
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.y) /
//        servo_period;
//    emcmotDebug[robotId]->teleop_data.desiredAccel.tran.z =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.tran.z -
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.z) /
//        servo_period;

//    /* a Carthesian Accel is computed */
//    pmCartMag(&emcmotDebug[robotId]->teleop_data.desiredAccel.tran,
//        &accell_mag);

//    /* then the accells for the rotary axes */
//    emcmotDebug[robotId]->teleop_data.desiredAccel.a =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.a -
//        emcmotDebug[robotId]->teleop_data.currentVel.a) /
//        servo_period;
//    emcmotDebug[robotId]->teleop_data.desiredAccel.b =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.b -
//        emcmotDebug[robotId]->teleop_data.currentVel.b) /
//        servo_period;
//    emcmotDebug[robotId]->teleop_data.desiredAccel.c =
//        (emcmotDebug[robotId]->teleop_data.desiredVel.c -
//        emcmotDebug[robotId]->teleop_data.currentVel.c) /
//        servo_period;
//    if (fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.a) > accell_mag) {
//        accell_mag = fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.a);
//    }
//    if (fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.b) > accell_mag) {
//        accell_mag = fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.b);
//    }
//    if (fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.c) > accell_mag) {
//        accell_mag = fabs(emcmotDebug[robotId]->teleop_data.desiredAccel.c);
//    }

//    /* accell_mag should now hold the max accell */

//    if (accell_mag > emcmotStatus[robotId]->acc) {
//        /* if accell_mag is too great, all need resizing */
//        pmCartScalMult(&emcmotDebug[robotId]->teleop_data.desiredAccel.tran,
//        emcmotStatus[robotId]->acc / accell_mag,
//        &emcmotDebug[robotId]->teleop_data.currentAccel.tran);
//        emcmotDebug[robotId]->teleop_data.currentAccel.a =
//        emcmotDebug[robotId]->teleop_data.desiredAccel.a *
//        emcmotStatus[robotId]->acc / accell_mag;
//        emcmotDebug[robotId]->teleop_data.currentAccel.b =
//        emcmotDebug[robotId]->teleop_data.desiredAccel.b *
//        emcmotStatus[robotId]->acc / accell_mag;
//        emcmotDebug[robotId]->teleop_data.currentAccel.c =
//        emcmotDebug[robotId]->teleop_data.desiredAccel.c *
//        emcmotStatus[robotId]->acc / accell_mag;
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.x +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.tran.x *
//        servo_period;
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.y +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.tran.y *
//        servo_period;
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.z +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.tran.z *
//        servo_period;
//        emcmotDebug[robotId]->teleop_data.currentVel.a +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.a *
//        servo_period;
//        emcmotDebug[robotId]->teleop_data.currentVel.b +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.b *
//        servo_period;
//        emcmotDebug[robotId]->teleop_data.currentVel.c +=
//        emcmotDebug[robotId]->teleop_data.currentAccel.c *
//        servo_period;
//    } else {
//        /* if accell_mag is not greater, the computed accell's stay as is */
//        emcmotDebug[robotId]->teleop_data.currentAccel =
//        emcmotDebug[robotId]->teleop_data.desiredAccel;
//        emcmotDebug[robotId]->teleop_data.currentVel =
//        emcmotDebug[robotId]->teleop_data.desiredVel;
//    }


//    /* based on curent position, current vel and period,
//       the next position is computed */
//    emcmotStatus[robotId]->carte_pos_cmd.tran.x +=
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.x *
//        servo_period;
//    emcmotStatus[robotId]->carte_pos_cmd.tran.y +=
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.y *
//        servo_period;
//    emcmotStatus[robotId]->carte_pos_cmd.tran.z +=
//        emcmotDebug[robotId]->teleop_data.currentVel.tran.z *
//        servo_period;
//    emcmotStatus[robotId]->carte_pos_cmd.a +=
//        emcmotDebug[robotId]->teleop_data.currentVel.a *
//        servo_period;
//    emcmotStatus[robotId]->carte_pos_cmd.b +=
//        emcmotDebug[robotId]->teleop_data.currentVel.b *
//        servo_period;
//    emcmotStatus[robotId]->carte_pos_cmd.c +=
//        emcmotDebug[robotId]->teleop_data.currentVel.c *
//        servo_period;

//    /* the next position then gets run through the inverse kins,
//        to compute the next positions of the joints */

//    /* OUTPUT KINEMATICS - convert to joints in local array */
//    emcmotConfig[robotId]->vtk->kinematicsInverse(&emcmotStatus[robotId]->carte_pos_cmd, positions,
//        &iflags, &fflags);
//    /* copy to joint structures and spline them up */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        joint->coarse_pos = positions[joint_num];
//        /* spline joints up-- note that we may be adding points
//           that fail soft limits, but we'll abort at the end of
//           this cycle so it doesn't really matter */
//        CubicClass::cubicAddPoint(&(joint->cubic), joint->coarse_pos);
//    }
//    /* END OF OUTPUT KINS */

//    /* there is data in the interpolators */
//    /* run interpolation */
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        /* save old command */
//        old_pos_cmd = joint->pos_cmd;
//        /* interpolate to get new one */
//        joint->pos_cmd = CubicClass::cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
//        joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
//    }

//    /* end of teleop mode */
//    break;

//    case EMCMOT_MOTION_DISABLED:
//    /* set position commands to match feedbacks, this avoids
//       disturbances and/or following errors when enabling */
//    emcmotStatus[robotId]->carte_pos_cmd = emcmotStatus[robotId]->carte_pos_fb;
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//        /* point to joint struct */
//        joint = &joints[joint_num];
//        /* save old command */
//        joint->pos_cmd = joint->pos_fb;
//        /* set joint velocity to zero */
//        joint->vel_cmd = 0.0;
//    }

//    break;
//    default:
//    break;
//    }
//    /* check command against soft limits */
//    /* This is a backup check, it should be impossible to command
//    a move outside the soft limits.  However there is at least
//    one case that isn't caught upstream: if an arc has both
//    endpoints inside the limits, but the curve extends outside,
//    the upstream checks will pass it.
//    */
//    onlimit = 0;
//    for (joint_num = 0; joint_num < num_joints; joint_num++) {
//    /* point to joint data */
//    joint = &joints[joint_num];
//    /* skip inactive or unhomed axes */
//    if (GET_JOINT_ACTIVE_FLAG(joint) && GET_JOINT_HOMED_FLAG(joint)) {
//        /* check for soft limits */
//        if (joint->pos_cmd > joint->max_pos_limit) {
//        onlimit = 1;
//                if (!emcmotStatus[robotId]->on_soft_limit)
//                    reportError(_("Exceeded positive soft limit on joint %d"), joint_num);
//        }
//        if (joint->pos_cmd < joint->min_pos_limit) {
//        onlimit = 1;
//                if (!emcmotStatus[robotId]->on_soft_limit)
//                    reportError(_("Exceeded negative soft limit on joint %d"), joint_num);
//        }
//    }
//    }
//    if ( onlimit ) {
//    if ( ! emcmotStatus[robotId]->on_soft_limit ) {
//        /* just hit the limit */
//        SET_MOTION_ERROR_FLAG(1);
//        emcmotStatus[robotId]->on_soft_limit = 1;
//    }
//    } else {
//    emcmotStatus[robotId]->on_soft_limit = 0;
//    }
}

/* NOTES:  These notes are just my understanding of how things work.

There are seven sets of position information.

1) emcmotStatus[robotId]->carte_pos_cmd
2) emcmotStatus[robotId]->joints[n].coarse_pos
3) emcmotStatus[robotId]->joints[n].pos_cmd
4) emcmotStatus[robotId]->joints[n].motor_pos_cmd
5) emcmotStatus[robotId]->joints[n].motor_pos_fb
6) emcmotStatus[robotId]->joints[n].pos_fb
7) emcmotStatus[robotId]->carte_pos_fb

Their exact contents and meaning are as follows:

1) This is the desired position, in Cartesean coordinates.  It is
   updated at the traj rate, not the servo rate.
   In coord mode, it is determined by the traj planner
   In teleop mode, it is determined by the traj planner?
   In free mode, it is not used, since free mode motion takes
     place in joint space, not cartesean space.  It may be
     displayed by the GUI however, so it is updated by
     applying forward kins to (2), unless forward kins are
     not available, in which case it is copied from (7).

2) This is the desired position, in joint coordinates, but
   before interpolation.  It is updated at the traj rate, not
   the servo rate..
   In coord mode, it is generated by applying inverse kins to (1)
   In teleop mode, it is generated by applying inverse kins to (1)
   In free mode, it is not used, since the free mode planner generates
     a new (3) position every servo period without interpolation.
     However, it is used indirectly by GUIs, so it is copied from (3).

3) This is the desired position, in joint coords, after interpolation.
   A new set of these coords is generated every servo period.
   In coord mode, it is generated from (2) by the interpolator.
   In teleop mode, it is generated from (2) by the interpolator.
   In free mode, it is generated by the simple free mode traj planner.

4) This is the desired position, in motor coords.  Motor coords are
   generated by adding backlash compensation, lead screw error
   compensation, and offset (for homing) to (3).
   It is generated the same way regardless of the mode, and is the
   output to the PID loop or other position loop.

5) This is the actual position, in motor coords.  It is the input from
   encoders or other feedback device (or from virtual encoders on open
   loop machines).  It is "generated" by reading the feedback device.

6) This is the actual position, in joint coordinates.  It is generated
   by subtracting offset, lead screw error compensation, and backlash
   compensation from (5).  It is generated the same way regardless of
   the operating mode.

7) This is the actual position, in Cartesean coordinates.  It is updated
   at the traj rate, not the servo rate.
   OLD VERSION:
   In the old version, there are four sets of code to generate actualPos.
   One for each operating mode, and one for when motion is disabled.
   The code for coord and teleop modes is identical.  The code for free
   mode is somewhat different, in particular to deal with the case where
   one or more axes are not homed.  The disabled code is quite similar,
   but not identical, to the coord mode code.  In general, the code
   calculates actualPos by applying the forward kins to (6).  However,
   where forward kins are not available, actualPos is either copied
   from (1) (assumes no following error), or simply left alone.
   These special cases are handled differently for each operating mode.
   NEW VERSION:
   I would like to both simplify and relocate this.  As far as I can
   tell, actualPos should _always_ be the best estimate of the actual
   machine position in Cartesean coordinates.  So it should always be
   calculated the same way.
   In addition to always using the same code to calculate actualPos,
   I want to move that code.  It is really a feedback calculation, and
   as such it belongs with the rest of the feedback calculations early
   in control.c, not as part of the output generation code as it is now.
   Ideally, actualPos would always be calculated by applying forward
   kinematics to (6).  However, forward kinematics may not be available,
   or they may be unusable because one or more axes aren't homed.  In
   that case, the options are: A) fake it by copying (1), or B) admit
   that we don't really know the Cartesean coordinates, and simply
   don't update actualPos.  Whatever approach is used, I can see no
   reason not to do it the same way regardless of the operating mode.
   I would propose the following:  If there are forward kins, use them,
   unless they don't work because of unhomed axes or other problems,
   in which case do (B).  If no forward kins, do (A), since otherwise
   actualPos would _never_ get updated.

*/

void EmcControlClass::compute_screw_comp(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    emcmot_comp_t *comp;
    double dpos;
    double a_max, v_max, v, s_to_go, ds_stop, ds_vel, ds_acc, dv_acc;


    /* compute the correction */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
        /* point to joint struct */
        joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
    if (!GET_JOINT_ACTIVE_FLAG(joint)) {
        /* if joint is not active, skip it */
        continue;
    }
    /* point to compensation data */
    comp = &(joint->comp);
    if ( comp->entries > 0 ) {
        /* there is data in the comp table, use it */
        /* first make sure we're in the right spot in the table */
        while ( joint->pos_cmd < comp->entry->nominal ) {
        comp->entry--;
        }
        while ( joint->pos_cmd >= (comp->entry+1)->nominal ) {
        comp->entry++;
        }
        /* now interpolate */
        dpos = joint->pos_cmd - comp->entry->nominal;
        if (joint->vel_cmd > 0.0) {
            /* moving "up". apply forward screw comp */
        joint->backlash_corr = comp->entry->fwd_trim +
                    comp->entry->fwd_slope * dpos;
        } else if (joint->vel_cmd < 0.0) {
            /* moving "down". apply reverse screw comp */
        joint->backlash_corr = comp->entry->rev_trim +
                    comp->entry->rev_slope * dpos;
        } else {
        /* not moving, use whatever was there before */
        }
    } else {
        /* no compensation data, just use +/- 1/2 of backlash */
        /** FIXME: this can actually be removed - if the user space code
        sends a single compensation entry with any nominal value,
        and with fwd_trim = +0.5 times the backlash value, and
        rev_trim = -0.5 times backlash, the above screw comp code
        will give exactly the same result as this code. */
        /* determine which way the compensation should be applied */
        if (joint->vel_cmd > 0.0) {
            /* moving "up". apply positive backlash comp */
        joint->backlash_corr = 0.5 * joint->backlash;
        } else if (joint->vel_cmd < 0.0) {
            /* moving "down". apply negative backlash comp */
        joint->backlash_corr = -0.5 * joint->backlash;
        } else {
        /* not moving, use whatever was there before */
        }
    }
    /* at this point, the correction has been computed, but
       the value may make abrupt jumps on direction reversal */
    /*
     * 07/09/2005 - S-curve implementation by Bas Laarhoven
     *
     * Implementation:
     *   Generate a ramped velocity profile for backlash or screw error comp.
     *   The velocity is ramped up to the maximum speed setting (if possible),
     *   using the maximum acceleration setting.
     *   At the end, the speed is ramped dowm using the same acceleration.
     *   The algorithm keeps looking ahead. Depending on the distance to go,
     *   the speed is increased, kept constant or decreased.
     *
     * Limitations:
     *   Since the compensation adds up to the normal movement, total
     *   accelleration and total velocity may exceed maximum settings!
     *   Currently this is limited to 150% by implementation.
     *   To fix this, the calculations in get_pos_cmd should include
     *   information from the backlash corection. This makes things
     *   rather complicated and it might be better to implement the
     *   backlash compensation at another place to prevent this kind
     *   of interaction.
     *   More testing under different circumstances will show if this
     *   needs a more complicate solution.
     *   For now this implementation seems to generate smoother
     *   movements and less following errors than the original code.
     */

    /* Limit maximum accelleration and velocity 'overshoot'
     * to 150% of the maximum settings.
     * The TP and backlash shouldn't use more than 100%
     * (together) but this requires some interaction that
     * isn't implemented yet.
     */
        v_max = 0.5 * joint->vel_limit * emcmotStatus[robotId]->net_feed_scale;
        a_max = 0.5 * joint->acc_limit;
        v = joint->backlash_vel;
        if (joint->backlash_corr >= joint->backlash_filt) {
            s_to_go = joint->backlash_corr - joint->backlash_filt; /* abs val */
            if (s_to_go > 0) {
                // off target, need to move
                ds_vel  = v * servo_period;           /* abs val */
                dv_acc  = a_max * servo_period;       /* abs val */
                ds_stop = 0.5 * (v + dv_acc) *
                        (v + dv_acc) / a_max; /* abs val */
                if (s_to_go <= ds_stop + ds_vel) {
                    // ramp down
                    if (v > dv_acc) {
                        // decellerate one period
                        ds_acc = 0.5 * dv_acc * servo_period; /* abs val */
                        joint->backlash_vel  -= dv_acc;
                        joint->backlash_filt += ds_vel - ds_acc;
                    } else {
                        // last step to target
                        joint->backlash_vel  = 0.0;
                        joint->backlash_filt = joint->backlash_corr;
                    }
                } else {
                    if (v + dv_acc > v_max) {
                        dv_acc = v_max - v;                /* abs val */
                    }
                    ds_acc  = 0.5 * dv_acc * servo_period; /* abs val */
                    ds_stop = 0.5 * (v + dv_acc) *
                                    (v + dv_acc) / a_max;  /* abs val */
                    if (s_to_go > ds_stop + ds_vel + ds_acc) {
                        // ramp up
                       joint->backlash_vel  += dv_acc;
                       joint->backlash_filt += ds_vel + ds_acc;
                    } else {
                       // constant velocity
                       joint->backlash_filt += ds_vel;
                    }
                }
            } else if (s_to_go < 0) {
                // safely handle overshoot (should not occur)
               joint->backlash_vel = 0.0;
               joint->backlash_filt = joint->backlash_corr;
            }
        } else {  /* joint->backlash_corr < 0.0 */
            s_to_go = joint->backlash_filt - joint->backlash_corr; /* abs val */
            if (s_to_go > 0) {
                // off target, need to move
                ds_vel  = -v * servo_period;          /* abs val */
                dv_acc  = a_max * servo_period;       /* abs val */
                ds_stop = 0.5 * (v - dv_acc) *
                    (v - dv_acc) / a_max; /* abs val */
                if (s_to_go <= ds_stop + ds_vel) {
                    // ramp down
                    if (-v > dv_acc) {
                        // decellerate one period
                        ds_acc = 0.5 * dv_acc * servo_period; /* abs val */
                        joint->backlash_vel  += dv_acc;   /* decrease */
                        joint->backlash_filt -= ds_vel - ds_acc;
                    } else {
                        // last step to target
                        joint->backlash_vel = 0.0;
                        joint->backlash_filt = joint->backlash_corr;
                    }
                } else {
                    if (-v + dv_acc > v_max) {
                        dv_acc = v_max + v;               /* abs val */
                    }
                    ds_acc = 0.5 * dv_acc * servo_period; /* abs val */
                    ds_stop = 0.5 * (v - dv_acc) *
                                    (v - dv_acc) / a_max; /* abs val */
                    if (s_to_go > ds_stop + ds_vel + ds_acc) {
                        // ramp up
                        joint->backlash_vel  -= dv_acc;   /* increase */
                        joint->backlash_filt -= ds_vel + ds_acc;
                    } else {
                        // constant velocity
                        joint->backlash_filt -= ds_vel;
                    }
                }
            } else if (s_to_go < 0) {
                // safely handle overshoot (should not occur)
                joint->backlash_vel = 0.0;
                joint->backlash_filt = joint->backlash_corr;
            }
        }
        /* backlash (and motor offset) will be applied to output later */
        /* end of joint loop */
    }
}

/*! \todo FIXME - once the HAL refactor is done so that metadata isn't stored
   in shared memory, I want to seriously consider moving some of the
   structures into the HAL memory block.  This will eliminate most of
   this useless copying, and make nearly everything accessible to
   halscope and halmeter for debugging.
*/

void EmcControlClass::output_to_hal(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    joint_hal_t *joint_data;
    static int old_motion_index=0, old_hal_index=0;

    /* output machine info to HAL for scoping, etc */

    *(emcmot_hal_data->motion_enabled) = GET_MOTION_ENABLE_FLAG(emcmotStatus[robotId]->motionFlag);
    *(emcmot_hal_data->in_position) = GET_MOTION_INPOS_FLAG(emcmotStatus[robotId]->motionFlag);
    *(emcmot_hal_data->coord_mode) = GET_MOTION_COORD_FLAG(emcmotStatus[robotId]->motionFlag);
    *(emcmot_hal_data->teleop_mode) = GET_MOTION_TELEOP_FLAG(emcmotStatus[robotId]->motionFlag);
    *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG(emcmotStatus[robotId]->motionFlag);
    *(emcmot_hal_data->on_soft_limit) = emcmotStatus[robotId]->on_soft_limit;
    if(emcmotStatus[robotId]->spindle.css_factor) {
    double denom = fabs(emcmotStatus[robotId]->spindle.xoffset - emcmotStatus[robotId]->carte_pos_cmd.tran.x);
    double speed;
        double maxpositive;
        if(denom > 0) speed = emcmotStatus[robotId]->spindle.css_factor / denom;
    else speed = emcmotStatus[robotId]->spindle.speed;

    speed = speed * emcmotStatus[robotId]->net_spindle_scale;

        maxpositive = fabs(emcmotStatus[robotId]->spindle.speed);
        // cap speed to G96 D...
        if(speed < -maxpositive)
            speed = -maxpositive;
        if(speed > maxpositive)
            speed = maxpositive;

    *(emcmot_hal_data->spindle_speed_out) = speed;
    *(emcmot_hal_data->spindle_speed_out_rps) = speed/60.;
    } else {
    *(emcmot_hal_data->spindle_speed_out) = emcmotStatus[robotId]->spindle.speed * emcmotStatus[robotId]->net_spindle_scale;
    *(emcmot_hal_data->spindle_speed_out_rps) = emcmotStatus[robotId]->spindle.speed * emcmotStatus[robotId]->net_spindle_scale / 60.;
    }
    *(emcmot_hal_data->spindle_speed_out_abs) = fabs(*(emcmot_hal_data->spindle_speed_out));
    *(emcmot_hal_data->spindle_speed_out_rps_abs) = fabs(*(emcmot_hal_data->spindle_speed_out_rps));
    *(emcmot_hal_data->spindle_speed_cmd_rps) = emcmotStatus[robotId]->spindle.speed / 60.;
    *(emcmot_hal_data->spindle_on) = ((emcmotStatus[robotId]->spindle.speed * emcmotStatus[robotId]->net_spindle_scale) != 0) ? 1 : 0;
    *(emcmot_hal_data->spindle_forward) = (*emcmot_hal_data->spindle_speed_out > 0) ? 1 : 0;
    *(emcmot_hal_data->spindle_reverse) = (*emcmot_hal_data->spindle_speed_out < 0) ? 1 : 0;
    *(emcmot_hal_data->spindle_brake) = (emcmotStatus[robotId]->spindle.brake != 0) ? 1 : 0;

    *(emcmot_hal_data->program_line) = emcmotStatus[robotId]->id;
    *(emcmot_hal_data->current_motion_type) = emcmotStatus[robotId]->motionType;
    *(emcmot_hal_data->distance_to_go) = emcmotStatus[robotId]->distance_to_go;

    if(GET_MOTION_COORD_FLAG(emcmotStatus[robotId]->motionFlag)) {
        *(emcmot_hal_data->current_vel) = emcmotStatus[robotId]->current_vel;
        *(emcmot_hal_data->requested_vel) = emcmotStatus[robotId]->requested_vel;
    } else if(GET_MOTION_TELEOP_FLAG(emcmotStatus[robotId]->motionFlag)) {
        PmCartesian t = emcmotDebug[robotId]->teleop_data.currentVel.tran;
        *(emcmot_hal_data->requested_vel) = 0.0;
        emcmotStatus[robotId]->current_vel = (*emcmot_hal_data->current_vel) = sqrt(t.x * t.x + t.y * t.y + t.z * t.z);
    } else {
        int i;
        double v2 = 0.0;
        for(i=0; i<num_joints; i++)
            if(GET_JOINT_ACTIVE_FLAG(&(joints[i+robotId*EMCMOT_MAX_JOINTS])) && joints[i+robotId*EMCMOT_MAX_JOINTS].free_tp_active)
                v2 += joints[i+robotId*EMCMOT_MAX_JOINTS].vel_cmd * joints[i+robotId*EMCMOT_MAX_JOINTS].vel_cmd;
        if(v2 > 0.0)
            emcmotStatus[robotId]->current_vel = (*emcmot_hal_data->current_vel) = sqrt(v2);
        else
            emcmotStatus[robotId]->current_vel = (*emcmot_hal_data->current_vel) = 0.0;
        *(emcmot_hal_data->requested_vel) = 0.0;
    }

    /* These params can be used to examine any internal variable. */
    /* Change the following lines to assign the variable you want to observe
       to one of the debug parameters.  You can also comment out these lines
       and copy elsewhere if you want to observe an automatic variable that
       isn't in scope here. */
    emcmot_hal_data->debug_bit_0 = joints[1+robotId*EMCMOT_MAX_JOINTS].free_tp_active;
    emcmot_hal_data->debug_bit_1 = emcmotStatus[robotId]->enables_new & AF_ENABLED;
    emcmot_hal_data->debug_float_0 = emcmotStatus[robotId]->net_feed_scale;
    emcmot_hal_data->debug_float_1 = emcmotStatus[robotId]->spindleRevs;
    emcmot_hal_data->debug_float_2 = emcmotStatus[robotId]->spindleSpeedIn;
    emcmot_hal_data->debug_float_3 = emcmotStatus[robotId]->net_spindle_scale;
    emcmot_hal_data->debug_s32_0 = emcmotStatus[robotId]->overrideLimitMask;
    emcmot_hal_data->debug_s32_1 = emcmotStatus[robotId]->tcqlen;

    /* two way handshaking for the spindle encoder */
    if(emcmotStatus[robotId]->spindle_index_enable && !old_motion_index) {
        *emcmot_hal_data->spindle_index_enable = 1;
    }

    if(!*emcmot_hal_data->spindle_index_enable && old_hal_index) {
        emcmotStatus[robotId]->spindle_index_enable = 0;
    }

    old_motion_index = emcmotStatus[robotId]->spindle_index_enable;
    old_hal_index = *emcmot_hal_data->spindle_index_enable;

    *(emcmot_hal_data->tooloffset_x) = emcmotStatus[robotId]->tool_offset.tran.x;
    *(emcmot_hal_data->tooloffset_y) = emcmotStatus[robotId]->tool_offset.tran.y;
    *(emcmot_hal_data->tooloffset_z) = emcmotStatus[robotId]->tool_offset.tran.z;
    *(emcmot_hal_data->tooloffset_a) = emcmotStatus[robotId]->tool_offset.a;
    *(emcmot_hal_data->tooloffset_b) = emcmotStatus[robotId]->tool_offset.b;
    *(emcmot_hal_data->tooloffset_c) = emcmotStatus[robotId]->tool_offset.c;
    *(emcmot_hal_data->tooloffset_u) = emcmotStatus[robotId]->tool_offset.u;
    *(emcmot_hal_data->tooloffset_v) = emcmotStatus[robotId]->tool_offset.v;
    *(emcmot_hal_data->tooloffset_w) = emcmotStatus[robotId]->tool_offset.w;

    /* output joint info to HAL for scoping, etc */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
    /* point to joint struct */
    joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
    /* apply backlash and motor offset to output */
    joint->motor_pos_cmd =
        joint->pos_cmd + joint->backlash_filt + joint->motor_offset;
    /* point to HAL data */
    joint_data = &(emcmot_hal_data->joint[joint_num]);
    /* write to HAL pins */
        *(joint_data->motor_offset) = joint->motor_offset;
    *(joint_data->motor_pos_cmd) = joint->motor_pos_cmd;
    *(joint_data->joint_pos_cmd) = joint->pos_cmd;
    *(joint_data->joint_pos_fb) = joint->pos_fb;
    *(joint_data->amp_enable) = GET_JOINT_ENABLE_FLAG(joint);
    *(joint_data->index_enable) = joint->index_enable;
    *(joint_data->homing) = GET_JOINT_HOMING_FLAG(joint);

    *(joint_data->coarse_pos_cmd) = joint->coarse_pos;
    *(joint_data->joint_vel_cmd) = joint->vel_cmd;
    *(joint_data->backlash_corr) = joint->backlash_corr;
    *(joint_data->backlash_filt) = joint->backlash_filt;
    *(joint_data->backlash_vel) = joint->backlash_vel;
    *(joint_data->f_error) = joint->ferror;
    *(joint_data->f_error_lim) = joint->ferror_limit;

    *(joint_data->free_pos_cmd) = joint->free_pos_cmd;
    *(joint_data->free_vel_lim) = joint->free_vel_lim;
    *(joint_data->free_tp_enable) = joint->free_tp_enable;
    *(joint_data->kb_jog_active) = joint->kb_jog_active;
    *(joint_data->wheel_jog_active) = joint->wheel_jog_active;

    *(joint_data->active) = GET_JOINT_ACTIVE_FLAG(joint);
    *(joint_data->in_position) = GET_JOINT_INPOS_FLAG(joint);
    *(joint_data->error) = GET_JOINT_ERROR_FLAG(joint);
    *(joint_data->phl) = GET_JOINT_PHL_FLAG(joint);
    *(joint_data->nhl) = GET_JOINT_NHL_FLAG(joint);
    *(joint_data->homed) = GET_JOINT_HOMED_FLAG(joint);
    *(joint_data->f_errored) = GET_JOINT_FERROR_FLAG(joint);
    *(joint_data->faulted) = GET_JOINT_FAULT_FLAG(joint);
    *(joint_data->home_state) = joint->home_state;
    }
}

void EmcControlClass::update_status(void)
{
    int joint_num, dio, aio;
    emcmot_joint_t *joint;
    emcmot_joint_status_t *joint_status;
#ifdef WATCH_FLAGS
    static int old_joint_flags[8];
    static int old_motion_flag;
#endif

    /* copy status info from private joint structure to status
       struct in shared memory */
    for (joint_num = 0; joint_num < num_joints; joint_num++) {
    /* point to joint data */
    joint = &joints[joint_num+robotId*EMCMOT_MAX_JOINTS];
    /* point to joint status */
    joint_status = &(emcmotStatus[robotId]->joint_status[joint_num]);
    /* copy stuff */
#ifdef WATCH_FLAGS
    /*! \todo FIXME - this is for debugging */
    if ( old_joint_flags[joint_num] != joint->flag ) {
        rtapi_print ( "Joint %d flag %04X -> %04X\n", joint_num, old_joint_flags[joint_num], joint->flag );
        old_joint_flags[joint_num] = joint->flag;
    }
#endif
    joint_status->flag = joint->flag;
    joint_status->pos_cmd = joint->pos_cmd;
    joint_status->pos_fb = joint->pos_fb;
    joint_status->vel_cmd = joint->vel_cmd;
    joint_status->ferror = joint->ferror;
    joint_status->ferror_high_mark = joint->ferror_high_mark;
    joint_status->backlash = joint->backlash;
    joint_status->max_pos_limit = joint->max_pos_limit;
    joint_status->min_pos_limit = joint->min_pos_limit;
    joint_status->min_ferror = joint->min_ferror;
    joint_status->max_ferror = joint->max_ferror;
//    joint_status->home_offset = *(joint->home_offset);
    }

    for (dio = 0; dio < num_dio; dio++) {
    emcmotStatus[robotId]->synch_di[dio] = *(emcmot_hal_data->synch_di[dio]);
    emcmotStatus[robotId]->synch_do[dio] = *(emcmot_hal_data->synch_do[dio]);
    }

    for (aio = 0; aio < num_aio; aio++) {
    emcmotStatus[robotId]->analog_input[aio] = *(emcmot_hal_data->analog_input[aio]);
    emcmotStatus[robotId]->analog_output[aio] = *(emcmot_hal_data->analog_output[aio]);
    }

    /*! \todo FIXME - the rest of this function is stuff that was apparently
       dropped in the initial move from emcmot.c to control.c.  I
       don't know how much is still needed, and how much is baggage.
    */

    /* motion emcmotDebug[robotId]->queue status */
    emcmotStatus[robotId]->depth = tpQueueDepth(emcmotQueue[robotId]);
    emcmotStatus[robotId]->activeDepth = tpActiveDepth(emcmotQueue[robotId]);

    // only update motion id and real depth when running on primary queue
    if (emcmotQueue[robotId] == emcmotPrimQueue[robotId]) {
    emcmotStatus[robotId]->depth = tpQueueDepth(emcmotQueue[robotId]);
    emcmotStatus[robotId]->id = tpGetExecId(emcmotQueue[robotId]);
    emcmotStatus[robotId]->tag = tpGetExecTag(&emcmotDebug[robotId]->tp);
    } else {
    // pretend we're doing something so task keeps
    // waiting for motion
    emcmotStatus[robotId]->depth = 1;
    }
    emcmotStatus[robotId]->pause_state = *(emcmot_hal_data->pause_state);
    emcmotStatus[robotId]->motionType = tpGetMotionType(emcmotQueue[robotId]);
    emcmotStatus[robotId]->queueFull = tcqFull(&emcmotQueue[robotId]->queue);

    /* check to see if we should pause in order to implement
       single emcmotDebug[robotId]->stepping */
    if (emcmotDebug[robotId]->stepping &&
    *emcmot_hal_data->pause_state == PS_RUNNING &&
    emcmotDebug[robotId]->idForStep != emcmotStatus[robotId]->id) {
    // defer the actual pause to the pause FSM since we're
    // switching motion queues - do this in one place only
    emcmotDebug[robotId]->stepping = 0;
    emcmotStatus[robotId]->pause_state = *(emcmot_hal_data->pause_state) = PS_PAUSING_FOR_STEP; // enter pause asap
    }
#ifdef WATCH_FLAGS
    /*! \todo FIXME - this is for debugging */
    if ( old_motion_flag != emcmotStatus[robotId]->motionFlag ) {
    rtapi_print ( "Motion flag %04X -> %04X\n", old_motion_flag, emcmotStatus[robotId]->motionFlag );
    old_motion_flag = emcmotStatus[robotId]->motionFlag;
    }
#endif
}

int EmcControlClass::emcmotController_test()
{
#ifdef D_USE_JOINT_INTERPOLATION
//    int tmpKey=get_pos_cmds_coordinateCubic(1);
//    int tmpKey=get_pos_cmds_coordinateSimpleTp(1);
    int tmpKey=get_pos_cmds_coordinateRescale(1);
#else
    int tmpKey=get_pos_cmds_coordinate(1);
#endif

    update_status();

    return tmpKey;
}
